commit 6350051e42d19a7a35b1e6bf62586ee87dba61bf
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Thu Feb 28 16:52:52 2013 -0500

    Added a script to auto-generate BehaviorList files.
    
    This includes some sub-changes:
     * Restructured behaviors/ folder into behaviors/, test_behaviors/ and limbo_behaviors/
     * Added a new CMakeLists.txt that automatically finds behaviors in their folders
     * Removed the obsolete trafficNavigtaion.*
     * Added a new generator to GUI.py that looks in the behaviors folder for the behaviors automatically.

diff --git a/scout/libscout/CMakeLists.txt b/scout/libscout/CMakeLists.txt
index ea24339..3c86d41 100644
--- a/scout/libscout/CMakeLists.txt
+++ b/scout/libscout/CMakeLists.txt
@@ -30,9 +30,9 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #target_link_libraries(example ${PROJECT_NAME})
 
 set(MAIN_FILES src/Sensors.cpp src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
-set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/line_follow.cpp src/behaviors/wl_test.cpp src/behaviors/pause_scout.cpp)
+FILE(GLOB BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/behaviors/*.cpp")
+FILE(GLOB TEST_BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/test_behaviors/*.cpp")
 
-set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp src/behaviors/maze_solve.cpp)
 set(HELPER_FILES src/helper_classes/Order.cpp src/helper_classes/PQWrapper.cpp)
 set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp)
 
diff --git a/scout/libscout/generate_behavior_lists.py b/scout/libscout/generate_behavior_lists.py
new file mode 100755
index 0000000..7b8412b
--- /dev/null
+++ b/scout/libscout/generate_behavior_lists.py
@@ -0,0 +1,90 @@
+#!/usr/bin/python
+
+# Usage:
+#
+# python generate_behavior_lists.py
+#
+# Parses to use only the final folder and file name for each source file.
+
+import sys
+import os
+
+AUTOGEN_HEADER = '''/**
+ ******************************************************************************
+ * WARNING: THIS IS AN AUTOGENERATED FILE!
+ *
+ * Editing this file is USELESS. It will be overwritten during the build.
+ * To properly edit this file, edit its corresponding *.template.* file, which
+ * a script run by CMake uses to generate this file.
+ *
+ * This file was generated via rules in src/generate_behavior_lists.py.
+ *
+ * Please see that file for a description of syntax and procedure for
+ * creating auto-generated files.
+ *
+ * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!
+ * THANK YOU.
+ *****************************************************************************
+ */
+
+'''
+
+# These are the codes this script will replace in template files.
+FULL_PATH_CODE = 'AUTOGEN_BEHAVIOR_FULL_PATH'
+LAST_PATH_CODE = 'AUTOGEN_BEHAVIOR_LAST_PATH'
+BASE_NAME_CODE = 'AUTOGEN_BEHAVIOR_BASE_NAME'
+
+# Different ways to refer to files in behaviors/ and test_behaviors/.
+full_paths = []
+last_paths = []
+base_names = []
+
+# Mapping of generated files to their templates
+R_FILES = {'src/BehaviorList.cpp' : 'src/BehaviorList.template.cpp',
+           'src/BehaviorList.h'   : 'src/BehaviorList.template.h'}
+
+# Parse the src/behaviors and src/test_behaviors folders
+cur_dir = os.getcwd()
+for path, dirname, fnames in os.walk('src'):
+    if path == 'src/behaviors' or path == 'src/test_behaviors':
+        path_parts = path.split('/')
+
+        for f in sorted(fnames):
+            # The pause_scout behavior needs to go first!
+            if f.endswith('.h') and 'pause_scout' in f:
+                full_paths.insert(0, os.path.join(cur_dir, path, f))
+                last_paths.insert(0, os.path.join(path_parts[-1], f))
+                base_names.insert(0, f.split('.')[0])
+            # Everything else goes in alphabetical order
+            elif f.endswith('.h'):
+                full_paths.append(os.path.join(cur_dir, path, f))
+                last_paths.append(os.path.join(path_parts[-1], f))
+                base_names.append(f.split('.')[0])
+
+# Now, a dictionary of codes to replace -> things to replace them with.
+replacements = {FULL_PATH_CODE : full_paths,
+                LAST_PATH_CODE : last_paths,
+                BASE_NAME_CODE : base_names}
+
+# Write all the replacements in R_FILES
+for outfile_name in R_FILES.keys():
+    with open(outfile_name, 'w') as outfile:
+        with open(R_FILES[outfile_name], 'r') as infile:
+            
+            # Write an obvious header so no one will edit our nicely
+            # generated file
+            outfile.write(AUTOGEN_HEADER)
+
+            for line in infile.readlines():
+                # If the line needs replacing, don't write the orgininal
+                # to the output file; rather, write many versions of the
+                # line replaced with each replacement value.
+                replaced = False
+                for r_key in replacements:
+                    if r_key in line:
+                        replaced = True
+                        for b in replacements[r_key]:
+                            outfile.write(line.replace(r_key, b))
+                # If it doesn't need replacing, just write the original
+                if not replaced:
+                    outfile.write(line)
diff --git a/scout/libscout/src/BehaviorList.cpp b/scout/libscout/src/BehaviorList.cpp
index 9b31faf..7bbb17b 100644
--- a/scout/libscout/src/BehaviorList.cpp
+++ b/scout/libscout/src/BehaviorList.cpp
@@ -1,17 +1,34 @@
-#include "BehaviorList.h"
+/**
+ ******************************************************************************
+ * WARNING: THIS IS AN AUTOGENERATED FILE!
+ *
+ * Editing this file is USELESS. It will be overwritten during the build.
+ * To properly edit this file, edit its corresponding *.template.* file, which
+ * a script run by CMake uses to generate this file.
+ *
+ * This file was generated via rules in src/generate_behavior_lists.py.
+ *
+ * Please see that file for a description of syntax and procedure for
+ * creating auto-generated files.
+ *
+ * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!
+ * THANK YOU.
+ *****************************************************************************
+ */
 
+#include "BehaviorList.h"
 
 BehaviorList::BehaviorList()
 {
   behavior_list.push_back(behavior<pause_scout>);
-  behavior_list.push_back(behavior<draw_cw_circle>);
-  behavior_list.push_back(behavior<draw_ccw_circle>);
   behavior_list.push_back(behavior<Odometry>);
+  behavior_list.push_back(behavior<draw_ccw_circle>);
+  behavior_list.push_back(behavior<draw_cw_circle>);
+  behavior_list.push_back(behavior<line_follow>);
   behavior_list.push_back(behavior<navigationMap>);
+  behavior_list.push_back(behavior<wl_test>);
   behavior_list.push_back(behavior<Scheduler>);
   behavior_list.push_back(behavior<WH_Robot>);
-  behavior_list.push_back(behavior<line_follow>);
-  behavior_list.push_back(behavior<wl_test>);
   behavior_list.push_back(behavior<maze_solve>);
   return;
 }
diff --git a/scout/libscout/src/BehaviorList.h b/scout/libscout/src/BehaviorList.h
index ecda221..fcd6143 100644
--- a/scout/libscout/src/BehaviorList.h
+++ b/scout/libscout/src/BehaviorList.h
@@ -1,18 +1,36 @@
+/**
+ ******************************************************************************
+ * WARNING: THIS IS AN AUTOGENERATED FILE!
+ *
+ * Editing this file is USELESS. It will be overwritten during the build.
+ * To properly edit this file, edit its corresponding *.template.* file, which
+ * a script run by CMake uses to generate this file.
+ *
+ * This file was generated via rules in src/generate_behavior_lists.py.
+ *
+ * Please see that file for a description of syntax and procedure for
+ * creating auto-generated files.
+ *
+ * THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION!
+ * THANK YOU.
+ *****************************************************************************
+ */
+
 #ifndef _BEHAVIOR_LIST_H_
 #define _BEHAVIOR_LIST_H_
 
 #include "Behavior.h"
-#include "behaviors/line_follow.h"
-#include "behaviors/draw_cw_circle.h"
-#include "behaviors/draw_ccw_circle.h"
+#include "Sensors.h"
+#include "behaviors/pause_scout.h"
 #include "behaviors/Odometry.h"
+#include "behaviors/draw_ccw_circle.h"
+#include "behaviors/draw_cw_circle.h"
+#include "behaviors/line_follow.h"
 #include "behaviors/navigationMap.h"
-#include "behaviors/Scheduler.h"
-#include "behaviors/WH_Robot.h"
 #include "behaviors/wl_test.h"
-#include "behaviors/pause_scout.h"
-#include "behaviors/maze_solve.h"
-#include "Sensors.h"
+#include "test_behaviors/Scheduler.h"
+#include "test_behaviors/WH_Robot.h"
+#include "test_behaviors/maze_solve.h"
 
 template<typename T> Behavior* behavior(std::string scoutname, Sensors* sensors){ return (Behavior*)new T(scoutname, sensors); } 
 
diff --git a/scout/libscout/src/behaviors/Scheduler.cpp b/scout/libscout/src/behaviors/Scheduler.cpp
deleted file mode 100644
index 443b1b1..0000000
--- a/scout/libscout/src/behaviors/Scheduler.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-#include "Scheduler.h"
-#include "../helper_classes/Order.h"
-
-using namespace std;
-
-/** @Brief: Initialize data structures for the scheduler. */
-Scheduler::Scheduler(std::string scoutname, Sensors* sensors):
-            Behavior(scoutname, "Scheduler", sensors)
-{
-  unassignedOrders = new PQWrapper(NUM_TASKS);
-
-	create_orders();
-}
-
-/** @Brief: Free allocatetd memory. */
-Scheduler::~Scheduler()
-{
-  delete unassignedOrders;
-}
-
-/** @Brief: Add robot to the waiting queue. 
- *  A robot calls this function with itself
- *  and gets pushed on a list of waiting robots.
- *  When a task is availaible the scheduler, removes
- *  the robot of the waiting queue, and gives it a
- *  task.
- */
-void Scheduler::get_task(int robot)
-{
-  Robot my_robot = robots[robot-1];
-  if(my_robot.sched_status != WAITING_ROBOT)
-  {
-	  waitingRobots.push(my_robot);
-    my_robot.sched_status = WAITING_ROBOT;
-  }
-}
-
-/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
-void Scheduler::create_orders()
-{
-  Path p;
-  p.len = 0;
-  p.path = NULL;
-  Time t = ros::Time::now();
-  Duration end(0);
-  Duration five(5);
-  Duration three(3);
-  Duration ten(10);
-	Order a(1,0,13,t+ten,p,end);
-  Order b(2,0,14,t+five,p,end);
-  Order c(3,0,15,t+three,p,end);
-  Order d(4,0,16,t,p,end);
-	
-	unassignedOrders->insert(a);
-	unassignedOrders->insert(b);
-	unassignedOrders->insert(c);
-	unassignedOrders->insert(d);
-}
-
-/** @Brief: This is a confirmation that the task is complete. 
-    This function removes the order from assignedOrders. */
-void Scheduler::task_complete(Order o)
-{
-  ROS_INFO("Scheduler: Task id %d was completed", o.getid());
-  for (unsigned int i=0; i<assignedOrders.size(); i++)
-  {
-    if (assignedOrders[i].getid()==o.getid())
-    {
-      assignedOrders.erase(assignedOrders.begin()+i);
-    }
-  }
-}
-
-/** @Brief: This is a confirmation that the task failed. 
-    This function places the order back on the PQWrapper. */
-void Scheduler::task_failed(Order o)
-{
-  ROS_INFO("Scheduler: Task id %d was failed", o.getid());
-	task_complete(o);
-	unassignedOrders->insert(o);
-}
-
-/** @Brief: Do a waiting dance. */
-void Scheduler::waiting_dance()
-{ 
-    //ROS_INFO("Scheduler: TEEHEE i do a dance!");
-}
-
-/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
-Order Scheduler::get_next_item()
-{
-  Time t = ros::TIME_MAX; 
-  double time = t.toSec();
-
-  Order* best;
-  int best_index;
-  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
-  {
-    Order order = unassignedOrders->peek(i);
-
-    Time end_time = order.get_start_time() - order.get_est_time();
-
-    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
-    {
-      best = &order;
-      best_index = i;
-      time = end_time.toSec() + MAX_WAIT_TIME;
-    }
-  }
-
-  Order ret;
-  ret = unassignedOrders->remove(best_index);
-  assignedOrders.push_back(ret);
-  return ret;
-}
-
-void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
-{
-  if(msg->data.compare(0, 6, "FAILED") == 0)
-  {
-    int order_id = atoi(msg->data.substr(7).c_str());
-    for(unsigned int i=0; i<assignedOrders.size(); i++)
-    {
-      if(assignedOrders[i].getid() == order_id)
-      {
-        task_failed(assignedOrders[i]);
-        break;
-      }
-    }
-  }
-  else if(msg->data.compare(0, 7, "SUCCESS") == 0)
-  {
-    int order_id = atoi(msg->data.substr(8).c_str());
-    for(unsigned int i=0; i<assignedOrders.size(); i++)
-    {
-      if(assignedOrders[i].getid() == order_id)
-      {
-        task_complete(assignedOrders[i]);
-        break;
-      }
-    }
-  }
-  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
-  {
-    ROS_INFO("SCHEDULER: got get task message");
-    int robot = atoi(msg->data.substr(9).c_str());
-    get_task(robot);
-  }
-  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 = robot_name;
-    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
-    new_robot.sched_status = NEW_ROBOT;
-    robots.push_back(new_robot);
-
-    std_msgs::String msg;
-    std::stringstream ss;
-    ss<<"REG_SUCCESS "<<id;
-    msg.data = ss.str();
-    new_robot.topic.publish(msg);
-    spinOnce();
-
-    ROS_INFO("Registration a success");
-
-    assert(robots.size() > 0);
-  }
-  else
-  {
-    ROS_INFO("I got a bad message: %s", msg->data.c_str());
-  }
-
-  return;
-}
-
-/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
-    this function calls the waiting_dance() function. */
-void Scheduler::run()
-{
-  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
-  ROS_INFO("I am a scheduler!. Now waiting for robots");
-  while(robots.size() < 1 && ok())
-  {
-    spinOnce();
-  }
-  ROS_INFO("SCHEDULER: main loop");
-	while (ok())
-	{
-    ROS_INFO("Scheduler running");
-    spinOnce();
-		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
-    {
-      waiting_dance();
-      spinOnce();
-    }
-  
-    ROS_INFO("SCHEDULER: Setting task");
-    Order next = get_next_item();
-    Robot r = waitingRobots.front();
-
-    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());
-    std_msgs::String msg;
-    std::stringstream ss;
-    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
-    std::cout<<"msg: "<<ss.str()<<endl;
-    msg.data = ss.str();
-    r.topic.publish(msg);
-  
-    spinOnce();
-
-    waitingRobots.front().sched_status = ORDERED_ROBOT;
-		waitingRobots.pop();
-	}
-}
-
-
-
-
diff --git a/scout/libscout/src/behaviors/Scheduler.h b/scout/libscout/src/behaviors/Scheduler.h
deleted file mode 100644
index 5e2bf28..0000000
--- a/scout/libscout/src/behaviors/Scheduler.h
+++ /dev/null
@@ -1,49 +0,0 @@
-#ifndef _SCHEDULER_
-#define _SCHEDULER_
-
-#include "../helper_classes/PQWrapper.h"
-#include "../helper_classes/Order.h"
-#include "../Behavior.h"
-
-#define NUM_TASKS 5
-#define WAITING_ROBOT 1
-#define NEW_ROBOT 2
-#define ORDERED_ROBOT 3
-
-typedef struct{
-  std::string name;
-  ros::Publisher topic;
-  int sched_status;
-} Robot;
-
-class Scheduler : Behavior {
-  std::vector<Robot> robots;
-	std::queue<Robot> waitingRobots;
-
-  PQWrapper* unassignedOrders;
-	std::vector<Order> assignedOrders;
-
-	void create_orders();
-
-	void waiting_dance();
-
-  void msg_callback(const std_msgs::String::ConstPtr& msg);
-
-public:
-  Scheduler(std::string scoutname, Sensors* sensors);
-	~Scheduler();
-	
-	void get_task(int robot);
-	
-	void task_complete(Order o);
-	void task_failed(Order o);
-	
-	
-	Order get_next_item();
-	
-	void run();
-
-  ros::Subscriber robot_to_sched;
-    
-};
-#endif
diff --git a/scout/libscout/src/behaviors/WH_Robot.cpp b/scout/libscout/src/behaviors/WH_Robot.cpp
deleted file mode 100644
index 17111ec..0000000
--- a/scout/libscout/src/behaviors/WH_Robot.cpp
+++ /dev/null
@@ -1,259 +0,0 @@
-#include "WH_Robot.h"
-#include "../helper_classes/Order.h"
-
-/** @Brief: warehouse robot constructor **/
-WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors):
-            line_follow(scoutname, sensors) 
-{
-  nav_map = new navigationMap(scoutname, sensors);
-  curr_task = DEFAULT_TASK;
-  name = scoutname;
-  reg_failed = 1;
-  srand(0xDEADBEEF);
-}
-
-WH_Robot::~WH_Robot()
-{
-  delete(nav_map);
-}
-
-Duration WH_Robot::get_worst_case_time(State start_state, State target_state)
-{
-  return nav_map->get_worst_case_time(start_state, target_state);
-}
-
-/** @brief Drives from state 2 to the robot home base and goes to sleep for
- *  x seconds
- */
-void WH_Robot::go_home(int x)
-{
-  unsigned int curr_state = nav_map->get_state();
-  if(curr_state != 2)
-  {
-    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
-    return;
-  }
-  turn_straight();
-  follow_line();
-  turn_left();
-  follow_line();
-  Duration sleep_time(x);
-  sleep_time.sleep();
-  return;
-}
-
-void WH_Robot::leave_home()
-{
-  unsigned int curr_state = nav_map->get_state();
-  if(curr_state != 2)
-  {
-    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
-    return;
-  }
-  turn_straight();
-  follow_line();
-  nav_map->update_state(ISTRAIGHT);
-  turn_left();
-  follow_line();
-}
-
-/** @brief Based on the given path, make
- *         the series of turns to follow this path,
- *         updating the current state in the navigation map as we do so   
- *  @param the Path to follow
- */
-void WH_Robot::follow_path(Path path_to_follow){
-  for(int i=0; i<path_to_follow.len; i++)
-  {
-    Turn t = path_to_follow.path[i];
-    unsigned int curr_state = nav_map->get_state();
-
-    nav_map->update_state(t);
-    switch(t)
-    {
-      case ISTRAIGHT:
-        turn_straight();
-        follow_line();
-        if(curr_state == 2)
-        {
-          turn_straight();
-          follow_line();
-          turn_straight();
-          follow_line();
-        }
-        break;
-      case ILEFT:
-        turn_left();
-        follow_line();
-        break;
-      case IRIGHT:
-        if(9 <= curr_state && curr_state <= 12)
-        {
-          turn_straight();
-          follow_line();
-        }
-        turn_right();
-        follow_line();
-        if(curr_state <= 4)
-        {
-          turn_straight();
-          follow_line();
-        }
-        break;
-      case IUTURN:
-        u_turn();
-        follow_line();
-        break;
-      case ISPOTTURN:
-        spot_turn();
-        follow_line();
-        break;
-    }
-  }  
-}
-
-int WH_Robot::exec_task()
-{
-  assert(curr_task != DEFAULT_TASK);
-  
-  // remember where you started doing this task so we know where to return
-  // State home_state = nav_map->get_state();
-  // For now, use state 2.
-  Path new_path = nav_map->shortest_path(curr_task->get_dest());
-  curr_task->set_path(new_path);
-
-  follow_path(new_path);
-   
-  //TODO: forklift yaaaay
-
-  int did_task_complete;
-
-  int error = rand() % 12;
-  if(error < 9) //Fail with 1/4 probability
-  {
-    ROS_INFO("WH_robot: TASK COMPLETE!");
-    did_task_complete = TASK_COMPLETED;
-  }
-  else
-  {
-    ROS_INFO("WH_robot: TASK FAILED!");
-    did_task_complete = TASK_FAILED;
-  }
-  
-  // For now use state 2 as home state
-  // Path return_path = nav_map->shortest_path(home_state);
-  Path return_path = nav_map->shortest_path(2);
-  curr_task->set_path(return_path);
-  
-  follow_path(return_path);
-
-  return did_task_complete;
-}
-
-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 = (char*)msg->data.c_str();
-    char* data;
-    data = strtok(string, " ");
-
-    data = strtok(NULL, " ");
-    int order_id = atoi(data);
-
-    data = strtok(NULL, " ");
-    Address source = atoi(data);
-
-    data = strtok(NULL, " ");
-    Address dest = atoi(data);
-
-    Time start_time = ros::Time::now();
-    Path path;
-    path.len = 0;
-    path.path = NULL;
-    Duration est_time(0);
-
-    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
-  }
-  else
-  {
-    ROS_INFO("I got a bad message: %s", msg->data.c_str());
-  }
-}
-
-void WH_Robot::run ()
-{
-  ROS_INFO("A WH_Robot has been created");
-
-  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("I am a robot. Registering with scheduler...");
-  while(reg_failed && ok())
-  {
-    std_msgs::String msg;
-    std::stringstream ss;
-    ss << "REGISTER "<<name;
-    msg.data = ss.str();
-    robot_to_sched.publish(msg);
-
-    spinOnce();
-  }
-
-  follow_line();
-
-  while(ok())
-  {
-    ROS_INFO("WH_ROBOT: Running main loop");
-
-
-      std_msgs::String msg;
-      std::stringstream ss;
-      ss << "GET_TASK "<<id;
-      msg.data = ss.str();
-      robot_to_sched.publish(msg);
-
-    while(curr_task == DEFAULT_TASK && ok()){
-      spinOnce();
-    }
-
-    int error = exec_task();
-
-    /** Go to robot home base */
-    go_home(2);
-    leave_home();
-
-    if(error == TASK_COMPLETED)
-    {
-      std_msgs::String msg;
-      std::stringstream ss;
-      ss << "SUCCESS "<<curr_task->getid();
-      msg.data = ss.str();
-      robot_to_sched.publish(msg);
-    }
-    else //error == TASK_FAILED
-    {
-      std_msgs::String msg;
-      std::stringstream ss;
-      ss << "FAILED "<<curr_task->getid();
-      msg.data = ss.str();
-      robot_to_sched.publish(msg);
-    }
-    delete curr_task;
-    curr_task = DEFAULT_TASK;
-
-  }
-}
-
-void WH_Robot::set_task(Order order)
-{
-  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
-      order.get_start_time(), order.get_path(), order.get_est_time()); 
-  return;
-}
-
diff --git a/scout/libscout/src/behaviors/WH_Robot.h b/scout/libscout/src/behaviors/WH_Robot.h
deleted file mode 100644
index f1427fc..0000000
--- a/scout/libscout/src/behaviors/WH_Robot.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#ifndef _WH_ROBOT_
-#define _WH_ROBOT_
-
-#define DEFAULT_TASK NULL
-#define TASK_COMPLETED 0
-#define TASK_FAILED -1
-
-#include "line_follow.h"
-#include "../Behavior.h"
-#include "navigationMap.h"
-#include "../helper_classes/Order.h"
-#include <assert.h>
-#include <stdlib.h>
-
-class WH_Robot : line_follow{
-        std::string name;
-        int id;
-
-        int reg_failed;
-
-        Order* curr_task;
-        navigationMap* nav_map;
-
-        Duration get_worst_case_time(State start_state, State target_state);
-        int exec_task();
-
-        void robot_callback(const std_msgs::String::ConstPtr& msg);
-
-        void go_home(int x);
-        void leave_home();
-
-    public:
-        WH_Robot(std::string scoutname, Sensors* sensors);
-        ~WH_Robot();
-        void run();
-
-        void set_task(Order order);
-        void follow_path(Path path_to_follow);
-
-        ros::Publisher robot_to_sched;
-        ros::Subscriber sched_to_robot;
-};
-
-#endif
diff --git a/scout/libscout/src/behaviors/maze_solve.cpp b/scout/libscout/src/behaviors/maze_solve.cpp
deleted file mode 100644
index b5e272b..0000000
--- a/scout/libscout/src/behaviors/maze_solve.cpp
+++ /dev/null
@@ -1,279 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- */
-
-#include "maze_solve.h"
-
-using namespace std;
-
-
-// want to have a minimal working thing, use a big enough 
-// static array and start in the middle
-// we assume we are facing right, that affects where we store
-// wall information
-// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical
-#define WALL -1
-#define UNSEEN 0
-#define SEEN 1
-#define CRITICAL 2
-// facings
-#define UP 0
-#define RIGHT 1
-#define DOWN 2
-#define LEFT 3
-
-Duration sonar_update_time(1.5);
-
-void maze_solve::run(){
-    
-    // TODO:first initialize map to all 0's
-    ROS_INFO("Starting to solve the maze");
-    // Go up to the first line.
-    follow_line();
-    // Turn the sonar on.
-    sonar->set_on();
-    sonar->set_range(0, 23);
-
-    // Wait for the sonar to initialize.
-    while(!look_around(25, 25, RIGHT) && ok())
-    {
-      spinOnce();      
-    }
-
-    // Solve the maze
-    bool finished = solve(25,25, RIGHT);
-
-    // Check and report final condition.
-    if (finished)
-        ROS_INFO("YAY! I have solved the maze");
-    else
-        ROS_INFO("NO! The maze is unsolvable");
-}
-
-bool maze_solve::solve(int row, int col, int dir)
-{
-    int initial_dir = dir;
-
-    ROS_INFO("I am at direction %d", dir);
-
-    // use backtracking to solve the maze
-    if (at_destination())
-        return true;
-
-    // Wait for sonar to update.
-    sonar_update_time.sleep();
-
-    // this function should fill the adjacent cells around me with
-    // wall's or paths
-    while(!look_around(row, col, dir) && ok())
-    {
-        spinOnce();
-    }
-
-    /* try go up */
-    if (map[row-1][col] != WALL && initial_dir != UP)
-    {
-    ROS_INFO("GOING UP!");
-        // Turn up.
-        turn_from_to(dir, UP);
-        follow_line();
-        // Solve recursively.
-        bool solved = solve(row-1, col, DOWN);
-        if (solved)
-        {
-            return solved;
-        }
-        else
-        {
-            //Update where we are.
-            dir = UP;
-        }
-    }
-    /* try right */
-    if (map[row][col+1] != WALL && initial_dir != RIGHT)
-    {
-    ROS_INFO("GOING RIGHT!");
-        // Turn right.
-        turn_from_to(dir, RIGHT);
-        follow_line();
-        // Solve recursively.
-        bool solved = solve(row, col+1, LEFT);
-        if (solved)
-        {
-            return solved;
-        }
-        else
-        {
-            //Update where we are.
-            dir = RIGHT;
-        }
-    }
-    /* try down */
-    if (map[row+1][col] != WALL && initial_dir != DOWN)
-    {
-    ROS_INFO("GOING DOWN!");
-        // Turn down.
-        turn_from_to(dir, DOWN);
-        follow_line();
-        // Solve recursively.
-        bool solved = solve(row+1, col, UP);
-        if (solved)
-        {
-            return solved;
-        }
-        else
-        {
-            //Update where we are.
-            dir = DOWN;
-        }
-    }
-    /* try left */
-    if (map[row][col-1] != WALL && initial_dir != LEFT)
-    {
-    ROS_INFO("GOING LEFT!");
-        // Turn down.
-        turn_from_to(dir, LEFT);
-        follow_line();
-        // Solve recursively.
-        bool solved = solve(row, col-1, RIGHT);
-        if (solved)
-        {
-            return solved;
-        }
-        else
-        {
-            //Update where we are.
-            dir = LEFT;
-        }
-    }
-
-    ROS_INFO("DEAD END FOUND, TURNING BACK.");
-    // we have exhausted all the options. This path is clearly a
-    // dead end. go back to where we come from and return false.
-    turn_from_to(dir, initial_dir);
-    follow_line();
-    return false;
-}
-
-// this function takes in the current direction and turns the scout
-// into it intended direction
-void maze_solve::turn_from_to(int current_dir, int intended_dir) {
-    switch ((4 + intended_dir - current_dir) % 4) 
-    {
-        case 0:
-            spot_turn();
-            break;
-        case 1:
-            turn_left();
-            break;
-        case 2:
-            turn_straight();
-            break;
-        case 3:
-            turn_right();
-            break;
-    }
-}
-
-bool maze_solve::look_around(int row, int col, int dir)
-{
-    // look around current place using sonar
-    // store whether or not
-    // there is a wall into the map
-    // stores at row col 2 if point is critical, 1 otherwise
-    
-    int* readings = sonar->get_sonar_readings();
-    spinOnce();
-
-/*
-    // Look to the left.
-    int left_distance = (readings[1] + readings[0] + readings[47])/3;
-    // Look to the front.
-    int front_distance = (readings[35] + readings[36] + readings[37])/3;
-    // Look to the right.
-    int right_distance = (readings[23] + readings[24] + readings[25])/3;
-*/
-    // Look to the left.
-    int left_distance = readings[0];
-    // Look to the front.
-    int front_distance = readings[36];
-    // Look to the right.
-    int right_distance = readings[24];
-
-    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);
-
-    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
-      return false;
-
-    switch (dir)
-    {
-        case UP:
-            // If the distance is less than 500, mark the area as a wall otherwise
-            // mark it as seen.
-            map[row][col+1] = (left_distance < 500)?WALL:SEEN;
-            map[row+1][col] = (front_distance < 500)?WALL:SEEN;
-            map[row][col-1] = (right_distance < 500)?WALL:SEEN;
-            break;
-        case RIGHT:
-            // If the distance is less than 500, mark the area as a wall otherwise
-            // mark it as seen.
-            map[row+1][col] = (left_distance < 500)?WALL:SEEN;
-            map[row][col-1] = (front_distance < 500)?WALL:SEEN;
-            map[row-1][col] = (right_distance < 500)?WALL:SEEN;
-            break;
-        case DOWN:
-            // If the distance is less than 500, mark the area as a wall otherwise
-            // mark it as seen.
-            map[row][col-1] = (left_distance < 500)?WALL:SEEN;
-            map[row-1][col] = (front_distance < 500)?WALL:SEEN;
-            map[row][col+1] = (right_distance < 500)?WALL:SEEN;
-            break;
-        case LEFT:
-            // If the distance is less than 500, mark the area as a wall otherwise
-            // mark it as seen.
-            map[row-1][col] = (left_distance < 500)?WALL:SEEN;
-            map[row][col+1] = (front_distance < 500)?WALL:SEEN;
-            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
-            break;
-    }
-
-    return true;
-}
-
-bool maze_solve::at_destination() 
-{
-    vector<uint32_t> readings = linesensor->query();
-    if ( readings[0] > 200 &&
-         readings[1] < 55 &&
-         readings[2] < 55 &&
-         readings[3] > 200 &&
-         readings[4] > 200 &&
-         readings[5] < 55 &&
-         readings[6] < 55 &&
-         readings[7] > 200 )
-    {
-        return true;
-    }
-    return false;
-}
diff --git a/scout/libscout/src/behaviors/maze_solve.h b/scout/libscout/src/behaviors/maze_solve.h
deleted file mode 100644
index 8768e17..0000000
--- a/scout/libscout/src/behaviors/maze_solve.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- */
-
-#ifndef _MAZE_SOLVE_H_
-#define _MAZE_SOLVE_H_
-
-#include "line_follow.h"
-
-class maze_solve: public line_follow
-{
-    public:
-        maze_solve(std::string scoutname, Sensors* sensors):
-            line_follow(scoutname, "maze_solve", sensors) {};
-        void run();
-    private:
-        bool solve(int row, int col, int dir);
-        void turn_from_to(int current_dir, int intended_dir);
-        bool look_around(int row, int col, int dir);
-        bool at_destination();
-
-        int map[60][60];
-};
-#endif
-
diff --git a/scout/libscout/src/behaviors/trafficNavigation.cpp b/scout/libscout/src/behaviors/trafficNavigation.cpp
deleted file mode 100644
index 4cabdf4..0000000
--- a/scout/libscout/src/behaviors/trafficNavigation.cpp
+++ /dev/null
@@ -1,431 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- */
-
-#include "trafficNavigation.h"
-#include "../linefollowing/lineDrive.h"
-
-#define DEBUG_INTERSECTION
-#ifdef MAIN_NEW
-
-#ifdef DEBUG_INTERSECTION
-  #define DBG_USBS(str) usb_puts(str)
-  #define DBG_USBI(x) usb_puti(x)
-#else
-  #define DBG_USBS(str)
-  #define DBG_USBI(x)
-#endif
-
-static int state, sign, turnDir;
-static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;
-unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;
-bool done;
-
-
-//TODO: classes for lineDrive::doDrive and other functions in line_drive.h, clock functions, sending/receiving packets, encoder data, initializations, get ids, etc.
-void trafficNavigation::run()
-{
-
-	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
-	
-	id = get_robotid();
-	sign = 0;
-	delay_ms(500);
-	
-	//Test code
-	state = SROAD;
-
-	sendBuffer[1] = id;
-
-	
-	while (ok()) {
-		/*DTN Finite State Machine*/
-		switch(state){
-		case SROAD:/*Following a normal road*/
-			start();
-			done = false;
-			
-			//finishes when reads a barcode: TODO: how do we detect intersections now?
-			while(!done){
-				sign = lineDrive::doDrive(180);
-				switch(sign){
-					case NORMAL:
-					case FINISHED:
-					case LOST:
-					case ERROR:
-						break;
-					default:
-						//we have a barcode!
-						state = SINTERSECTION_ENTER;
-						DBG_USBS("Read Barcode #:");
-						DBG_USBI(sign);
-						DBG_USBS("\n");
-						done = true;
-						break;
-
-				}
-			}
-			break;
-		case SINTERSECTION_ENTER:/*Entering, and in intersection*/
-			stop();
-			lineDrive::doDrive(0);
-
-			DBG_USBS("STATE: SINTERSECTION_ENTER\n");
-
-			/*Intersection queue:
-			 *Each robot when entering the intersection will check for other robots
-			 *in the intersection, and insert itself in a queue to go through.
-			 */
-			queuePrevBot = -1; //the bot that will drive before this bot
-			queueNextBot = -1; //the bot that will drive after this bot
-			resolvPrevBotID = -3; //in the case of a race, the bot that is
-			                      //to enter the queue before this bot
-			resolvPrevBotDraw = 0; //the random priority number that bot has
-			resolvDraw = 0; //my random priority number
-			
-			intersectionNum = getIntersectNum(sign);
-			if(intersectionNum == (char) -1){ //invalid
-				state = SROAD;
-				DBG_USBS("Barcode has invalid intersectionNum\n");
-				break;
-			}
-			turnDir = validateTurn(sign, getTurnType(sign));
-			if(turnDir == (char) -1){ //invalid
-				state = SROAD;
-				DBG_USBS("Barcode has invalid turn\n");
-				break;
-			}
-
-			trafficNavigation::enterIntersection(); //sends wireless packet for entry
-			state = SINTERSECTION_WAIT;
-
-			rtc_reset(); //reset rtc for timeout wait for reply
-			done = false;
-			char retried = 0;
-			while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue
-				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);
-				if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier...
-					trafficNavigation::enterIntersection();
-					retried = 1;
-				}
-				switch (ret) {
-					case KPLACEDINQUEUE:
-						done = true;
-						break;
-					case KFAILEDTOQUEUE:
-						DBG_USBS("Failed to queue\n");
-						trafficNavigation::enterIntersection();
-						rtc_reset();
-						break;
-					case KRESOLVINGENTER:
-						state = SINTERSECTION_ENTER_RESOLV;
-						done = true;
-						break;
-				}
-			}
-			break;
-
-		case SINTERSECTION_ENTER_RESOLV:
-			DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n");
-
-			rtc_reset();
-			done = false;
-			retried = 0;
-			while(rtc_get() < 4 && !done){
-				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);
-				switch (ret) {
-					case KRESOLVINGENTER:
-						break;
-					case KPLACEDINQUEUE:
-						done = true;
-						break;
-					case KFAILEDTOQUEUE:
-						DBG_USBS("Failed to queue\n");
-						trafficNavigation::enterIntersection();
-						rtc_reset();
-						break;
-				}
-				//if resolvPrevBotID == -1, this indicates that
-				//there was a prevbot before, but it has entered
-				//the queue so it's our turn.
-				if(!done && resolvPrevBotID == (char) -1 && !retried){
-					trafficNavigation::enterIntersection();
-					rtc_reset();
-					retried = 1;
-				//if resolvPrevBotID == -2, we have been
-				//resolving but never have seen a bot with lower
-				//priority than us. after the 6/16ths sec
-				//timeout, assume we are first.
-				} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){
-					//send a intersection reply to myself to
-					//trigger other bots to enter queue.
-					sendBuffer[0] = WINTERSECTIONREPLY;
-					sendBuffer[2] = intersectionNum;
-					sendBuffer[3] = id;
-					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
-
-					done = true;
-					break;
-				}
-			}
-			state = SINTERSECTION_WAIT;
-			break;
-		case SINTERSECTION_WAIT:/*Waiting in intersection */
-
-
-			DBG_USBS("STATE: SINTERSECTION_WAIT\n");
-
-			done = false;
-			while(queuePrevBot != (char) -1){
-				done = true;
-				int ret = wirelessPacketHandle(state);
-				if (ret==KFIRSTINQUEUE) state = SINTERSECTION_DRIVE;
-			}
-			//hack to make sure bot that just left intersection is
-			//really out of the intersection.
-			rtc_reset();
-			while(rtc_get() < 2 && done){//wait one second
-				wirelessPacketHandle(state);
-			}
-
-			state = SINTERSECTION_DRIVE;
-			break;
-		case SINTERSECTION_DRIVE:
-			DBG_USBS("STATE: SINTERSECTION_DRIVE\n");
-			start();
-			turn(getIntersectType(sign), turnDir);
-			
-			
-			//Exits intersection
-			sendBuffer[0] = WINTERSECTIONEXIT;
-			sendBuffer[2] = intersectionNum;//Intersection #
-			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
-
-			state = SROAD;
-			break;
-		default:
-			DBG_USBS("I got stuck in an unknown state (Likely highway) My state is ");
-			DBG_USBI(state);
-		}
-	}
-}
-
-int trafficNavigation::wirelessPacketHandle(int state){
-	int dataLength = 0;
-	unsigned char *packet = NULL;
-	packet = wl_basic_do_default(&dataLength);
-	
-	/* sanity check */
-	if(dataLength == 0 || packet == NULL) //no packet
-		return ENOPACKET;
-
-	if(dataLength != PACKET_LENGTH)
-		return EPACKETLEN;
-
-	DBG_USBS("Recieved Wireless Packet: ");
-	for(int i = 0; i < dataLength; i++){
-		DBG_USBI(packet[i]);
-		DBG_USBS(" ");
-	}
-	DBG_USBS("\n");
-
-	switch (packet[0]){
-		case WROADENTRY: //[type, bot, road]
-			return ENOACTION;
-			break;
-		case WROADEXIT: //[type, bot, road]
-			return ENOACTION;
-			break;
-		case WROADSTOP: //[type, bot, road]
-			return ENOACTION;
-			break;
-		case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]
-			if (packet[2] == intersectionNum){
-				switch (state){
-					case SINTERSECTION_ENTER:
-						trafficNavigation::sendResolv(false);
-						resolvPrevBotID = -2;
-						return KRESOLVINGENTER;
-						break;
-					case SINTERSECTION_ENTER_RESOLV:
-						return ENOACTION;
-					case SINTERSECTION_WAIT:
-					case SINTERSECTION_DRIVE:
-						if(queueNextBot == (char) -1){
-							sendBuffer[0] = WINTERSECTIONREPLY;
-							sendBuffer[2] = intersectionNum;
-							sendBuffer[3] = packet[1];
-							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
-							queueNextBot = packet[1];
-							return KREPLIEDTOENTER;
-						}
-						break;
-
-				}
-			}
-			break;
-		case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]
-			if (packet[2] == intersectionNum){
-				switch (state){
-					case SINTERSECTION_ENTER:
-					case SINTERSECTION_ENTER_RESOLV:
-						if(packet[3] == id){ //Reply for me
-							queuePrevBot = packet[1];
-							return KPLACEDINQUEUE;
-						} else {
-							if(packet[3] == resolvPrevBotID)
-								resolvPrevBotID = -1;
-							return KFAILEDTOQUEUE;
-						}
-						break;
-					default:
-						return ENOACTION;
-				}
-			}
-			break;
-		case WINTERSECTIONEXIT: //[type, bot, intersection]
-			if (packet[2] == intersectionNum){
-				switch (state){
-					case SINTERSECTION_WAIT:
-						if(packet[1]==queuePrevBot){
-							queuePrevBot=-1;
-							return KFIRSTINQUEUE;
-						}
-				}
-			}
-			break;
-		case WINTERSECTIONGO: //[type, bot, intersection]
-			break;
-		case WINTERSECTIONPOLICEENTRY: //[?]
-			return ENOACTION;
-			break;
-		case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]
-			//in the case robots draw the same number these will be
-			//arbitrated using the sending robot's id, prefering
-			//lower ids
-			DBG_USBS("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");
-			DBG_USBI((int) resolvPrevBotID);
-			DBG_USBS("\n");
-			if (packet[2] == intersectionNum){
-				switch (state){
-					case SINTERSECTION_ENTER:
-					case SINTERSECTION_ENTER_RESOLV:
-						if(resolvPrevBotID == (char) -3){
-							DBG_USBS("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");
-							trafficNavigation::sendResolv(false);
-							resolvPrevBotID = -2;
-						}
-						if((unsigned char) packet[3] == resolvDraw && id > packet[1]){
-							//other bot drew same number as me,
-							//and i have a higher id, so it goes first.
-							DBG_USBS("bot ");
-							DBG_USBI(packet[1]);
-							DBG_USBS(" Drew the same number as me and I have a higher id, so it goes first\n");
-							resolvPrevBotID = packet[1];
-						} else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){
-							//other bot drew same number as the bot before me,
-							//so if it has a higher id than the bot before me,
-							//it is going to go in between that one and me.
-							DBG_USBS("bot ");
-							DBG_USBI(packet[1]);
-							DBG_USBS(" Drew the same number as the bot before me, and the bot before me has have a higher id, so this goes first\n");
-							resolvPrevBotID = packet[1];
-						} else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){
-							//found a bot that goes in between the bot before me
-							//and me, so now it is before me.
-							DBG_USBS("bot ");
-							DBG_USBI(packet[1]);
-							DBG_USBS(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n");
-							resolvPrevBotDraw = packet[3];
-							resolvPrevBotID = packet[1];
-						}
-						return KRESOLVINGENTER;
-						break;
-					case SINTERSECTION_WAIT:
-					case SINTERSECTION_DRIVE:
-						DBG_USBS("Trying to resolv in non resolv case...queueNextbot = "); DBG_USBI(queueNextBot); DBG_USBS("\n");
-						if(queueNextBot == (char) -1){
-							trafficNavigation::sendResolv(true);
-						}
-						return KRESOLVINGENTER;
-						break;
-				}
-				
-			}
-			break;
-		case WHIGHWAYENTRY: //[type, bot, highway]
-			return ENOACTION;
-			break;
-		case WHIGHWAYREPLY: //[type, fromBot, highway, toBot]
-			return ENOACTION;
-			break;
-		case WHIGHWAYEXIT: //[type, bot, highway]
-			return ENOACTION;
-			break;
-		case WPINGGLOBAL: //[type, bot]
-			return ENOACTION;
-			break;
-		case WPINGBOT: //[type, fromBot, toBot]
-			return ENOACTION;
-			break;
-		case WPINGQUEUE: //[type, fromBot, toBot]
-			return ENOACTION;
-			break;
-		case WPINGREPLY: //[type, fromBot, toBot]
-			return ENOACTION;
-			break;
-		case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces
-			return ENOACTION;
-			break;
-		default:
-			return ENOACTION;
-			break;
-	}
-}
-
-unsigned char trafficNavigation::resolvRandomNumberGen(){
-	if ((resolvSeed *= (rtc_get() + encoder_read(LEFT))%9) == 0){
-		return resolvSeed + 1; //0 is a reseved priority value for the last
-		                 //bot that is already in the queue.
-	}
-	return resolvSeed;
-}
-void trafficNavigation::sendResolv(bool override){
-	if(!override)
-		resolvDraw = trafficNavigation::resolvRandomNumberGen();
-	else
-		resolvDraw = 0;
-	sendBuffer[0] = WINTERSECTIONRESOLVERACE;
-	sendBuffer[2] = intersectionNum;
-	sendBuffer[3] = resolvDraw;
-	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
-}
-void trafficNavigation::enterIntersection(void){
-	//Sends packet announcing its entry to the intersection
-	sendBuffer[0] = WINTERSECTIONENTRY;
-	sendBuffer[2] = intersectionNum;//Intersection #
-	sendBuffer[3] = 0; //getIntersectPos(sign);
-	sendBuffer[4] = turnDir;
-	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
-}
diff --git a/scout/libscout/src/behaviors/trafficNavigation.h b/scout/libscout/src/behaviors/trafficNavigation.h
deleted file mode 100644
index 8d2a6db..0000000
--- a/scout/libscout/src/behaviors/trafficNavigation.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/**
- * Copyright (c) 2011 Colony Project
- * 
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use,
- * copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following
- * conditions:
- * 
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
- * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
- * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
- * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- */
-
-#ifndef _trafficNavigation_H_
-#define _trafficNavigation_H_
-
-#include "../Behavior.h"
-
-//from old trafficNavigation.h
-#ifndef trafficNavigation_H
-
-/* Debug Options - These must go before includes */
-#define MAIN_NEW
-//#define SENDGRAPH
-
-#include <dragonfly_lib.h>
-#include <wl_basic.h>
-#include "../linefollowing/lineDrive.h"
-#include "highways.h"
-#include "intersectData.h"
-#include "validTurns.h"
-
-/*States*/
-#define SROAD 0
-#define SINTERSECTION 9 /* for old version */
-#define SINTERSECTION_ENTER 10
-#define SINTERSECTION_ENTER_RESOLV 11
-#define SINTERSECTION_WAIT 12
-#define SINTERSECTION_DRIVE 13
-#define SHIGHWAY 20
-#define SCLEARPACKET 30
-
-/*Sign Codes
- * bitwise OR labels to create a barcode or read one
- * There should be macros to extract these probably
- * The bits will be stored in some variable (char or short)
- * Bits if road: ? ? ? NAME NAME NAME TYPE CROAD
- * Bits if intersection: ? ? ? ? DIR DIR #WAYS CINTERSECTION
- */
-#define CROAD 0x0 //0b
-#define CINTERSECTION 0x1 //1b
-#define CNORMALROAD 0x0 //00b
-#define CHIGHWAYROAD 0x2 //10b
-#define C4WAY 0x0 //00b
-#define C3WAY 0x2 //10b
-#define CNORTH 0x0 //0000b
-#define CEAST 0x4 //0100b
-#define CSOUTH 0x8 //1000b
-#define CWEST 0x12 //1100b
-
-/*Wireless Packet Types
- * The first byte of any wireless packet should be one of these types.
- * Each type will have its own structure
- * The second byte should be the id of the bot sending the packet
- * The third byte should be the number of the intersection or road that
- *   the packet pertains to
- */
-#define PACKET_LENGTH 5
-#define WROADENTRY 0 //[type, bot, road]
-#define WROADREPLY 1 //[type, fromBot, road, toBot]
-#define WROADEXIT 2 //[type, bot, road]
-#define WROADSTOP 3 //[type, bot, road]
-#define WINTERSECTIONENTRY 10   //[type, bot, intersection, fromDir, toDir]
-#define WINTERSECTIONREPLY 11   //[type, fromBot, intersection, toBot]
-#define WINTERSECTIONEXIT 12    //[type, bot, intersection]
-#define WINTERSECTIONGO 13      //[type, bot, intersection]
-#define WINTERSECTIONPOLICEENTRY 14
-#define WINTERSECTIONRESOLVERACE 15   //[type, bot, intersection, num]
-#define WHIGHWAYENTRY 20 //[type, bot, highway]
-#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]
-#define WHIGHWAYEXIT 22 //[type, bot, highway]
-#define WPINGGLOBAL 30 //[type, bot]
-#define WPINGBOT 31 //[type, fromBot, toBot]
-#define WPINGQUEUE 32 //[type, fromBot, toBot]
-#define WPINGREPLY 33 //[type, fromBot, toBot]
-#define WCOLLISIONAVOID 41 //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces
-#define WGRAPHDATA 80
-
-/*Wireless Parsing Status
- * For wireless parsing - status codes describing various errors/statuses with wireless
- * parsing. 
- * Errors begin with E - Note that all of these errors are actually returned as negative.
- *                     - values of >= 100 indicate a serious error
- * Non-Errors begin with K - returned as positive. (don't start with S because S
- *                           is for state)
- */
-#define ENOPACKET -10 //The received packet doesn't exist.
-#define ENOACTION -15 //The received packet has no defined action, and this is OK
-#define ENOIMPLEMENT -20 //The received packet has no defined action and needs to be implemented.
-#define EPACKETLEN -100 //The received packet was the wrong length
-#define KOK 1
-#define KPLACEDINQUEUE 20
-#define KFAILEDTOQUEUE 21
-#define KFIRSTINQUEUE 22
-#define KREPLIEDTOENTER 23
-#define KRESOLVINGENTER 24 //resolving a race between bots trying to enter intersection at the same time.
-
-/*Macros
- */
-#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE)
-
-#endif
-//
-
-#ifdef DEBUG_INTERSECTION
-  #define DBG_USBS(str) usb_puts(str)
-  #define DBG_USBI(x) usb_puti(x)
-#else
-  #define DBG_USBS(str)
-  #define DBG_USBI(x)
-#endif
-
-
-class trafficNavigation : Behavior
-{
-    public:
-        trafficNavigation(std::string scoutname, Sensors* sensors) : 
-                Behavior(scoutname, "traffic_nav", sensors) {};
-
-        void run();
-		int wirelessPacketHandle(int state);
-		void enterIntersection(void);
-		void sendResolv(bool override);
-		unsigned char resolvRandomNumberGen();
-};
-
-#endif
diff --git a/scout/libscout/src/test_behaviors/Scheduler.cpp b/scout/libscout/src/test_behaviors/Scheduler.cpp
new file mode 100644
index 0000000..443b1b1
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/Scheduler.cpp
@@ -0,0 +1,228 @@
+#include "Scheduler.h"
+#include "../helper_classes/Order.h"
+
+using namespace std;
+
+/** @Brief: Initialize data structures for the scheduler. */
+Scheduler::Scheduler(std::string scoutname, Sensors* sensors):
+            Behavior(scoutname, "Scheduler", sensors)
+{
+  unassignedOrders = new PQWrapper(NUM_TASKS);
+
+	create_orders();
+}
+
+/** @Brief: Free allocatetd memory. */
+Scheduler::~Scheduler()
+{
+  delete unassignedOrders;
+}
+
+/** @Brief: Add robot to the waiting queue. 
+ *  A robot calls this function with itself
+ *  and gets pushed on a list of waiting robots.
+ *  When a task is availaible the scheduler, removes
+ *  the robot of the waiting queue, and gives it a
+ *  task.
+ */
+void Scheduler::get_task(int robot)
+{
+  Robot my_robot = robots[robot-1];
+  if(my_robot.sched_status != WAITING_ROBOT)
+  {
+	  waitingRobots.push(my_robot);
+    my_robot.sched_status = WAITING_ROBOT;
+  }
+}
+
+/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
+void Scheduler::create_orders()
+{
+  Path p;
+  p.len = 0;
+  p.path = NULL;
+  Time t = ros::Time::now();
+  Duration end(0);
+  Duration five(5);
+  Duration three(3);
+  Duration ten(10);
+	Order a(1,0,13,t+ten,p,end);
+  Order b(2,0,14,t+five,p,end);
+  Order c(3,0,15,t+three,p,end);
+  Order d(4,0,16,t,p,end);
+	
+	unassignedOrders->insert(a);
+	unassignedOrders->insert(b);
+	unassignedOrders->insert(c);
+	unassignedOrders->insert(d);
+}
+
+/** @Brief: This is a confirmation that the task is complete. 
+    This function removes the order from assignedOrders. */
+void Scheduler::task_complete(Order o)
+{
+  ROS_INFO("Scheduler: Task id %d was completed", o.getid());
+  for (unsigned int i=0; i<assignedOrders.size(); i++)
+  {
+    if (assignedOrders[i].getid()==o.getid())
+    {
+      assignedOrders.erase(assignedOrders.begin()+i);
+    }
+  }
+}
+
+/** @Brief: This is a confirmation that the task failed. 
+    This function places the order back on the PQWrapper. */
+void Scheduler::task_failed(Order o)
+{
+  ROS_INFO("Scheduler: Task id %d was failed", o.getid());
+	task_complete(o);
+	unassignedOrders->insert(o);
+}
+
+/** @Brief: Do a waiting dance. */
+void Scheduler::waiting_dance()
+{ 
+    //ROS_INFO("Scheduler: TEEHEE i do a dance!");
+}
+
+/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
+Order Scheduler::get_next_item()
+{
+  Time t = ros::TIME_MAX; 
+  double time = t.toSec();
+
+  Order* best;
+  int best_index;
+  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
+  {
+    Order order = unassignedOrders->peek(i);
+
+    Time end_time = order.get_start_time() - order.get_est_time();
+
+    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
+    {
+      best = &order;
+      best_index = i;
+      time = end_time.toSec() + MAX_WAIT_TIME;
+    }
+  }
+
+  Order ret;
+  ret = unassignedOrders->remove(best_index);
+  assignedOrders.push_back(ret);
+  return ret;
+}
+
+void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
+{
+  if(msg->data.compare(0, 6, "FAILED") == 0)
+  {
+    int order_id = atoi(msg->data.substr(7).c_str());
+    for(unsigned int i=0; i<assignedOrders.size(); i++)
+    {
+      if(assignedOrders[i].getid() == order_id)
+      {
+        task_failed(assignedOrders[i]);
+        break;
+      }
+    }
+  }
+  else if(msg->data.compare(0, 7, "SUCCESS") == 0)
+  {
+    int order_id = atoi(msg->data.substr(8).c_str());
+    for(unsigned int i=0; i<assignedOrders.size(); i++)
+    {
+      if(assignedOrders[i].getid() == order_id)
+      {
+        task_complete(assignedOrders[i]);
+        break;
+      }
+    }
+  }
+  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
+  {
+    ROS_INFO("SCHEDULER: got get task message");
+    int robot = atoi(msg->data.substr(9).c_str());
+    get_task(robot);
+  }
+  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 = robot_name;
+    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
+    new_robot.sched_status = NEW_ROBOT;
+    robots.push_back(new_robot);
+
+    std_msgs::String msg;
+    std::stringstream ss;
+    ss<<"REG_SUCCESS "<<id;
+    msg.data = ss.str();
+    new_robot.topic.publish(msg);
+    spinOnce();
+
+    ROS_INFO("Registration a success");
+
+    assert(robots.size() > 0);
+  }
+  else
+  {
+    ROS_INFO("I got a bad message: %s", msg->data.c_str());
+  }
+
+  return;
+}
+
+/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
+    this function calls the waiting_dance() function. */
+void Scheduler::run()
+{
+  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
+  ROS_INFO("I am a scheduler!. Now waiting for robots");
+  while(robots.size() < 1 && ok())
+  {
+    spinOnce();
+  }
+  ROS_INFO("SCHEDULER: main loop");
+	while (ok())
+	{
+    ROS_INFO("Scheduler running");
+    spinOnce();
+		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
+    {
+      waiting_dance();
+      spinOnce();
+    }
+  
+    ROS_INFO("SCHEDULER: Setting task");
+    Order next = get_next_item();
+    Robot r = waitingRobots.front();
+
+    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());
+    std_msgs::String msg;
+    std::stringstream ss;
+    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
+    std::cout<<"msg: "<<ss.str()<<endl;
+    msg.data = ss.str();
+    r.topic.publish(msg);
+  
+    spinOnce();
+
+    waitingRobots.front().sched_status = ORDERED_ROBOT;
+		waitingRobots.pop();
+	}
+}
+
+
+
+
diff --git a/scout/libscout/src/test_behaviors/Scheduler.h b/scout/libscout/src/test_behaviors/Scheduler.h
new file mode 100644
index 0000000..5e2bf28
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/Scheduler.h
@@ -0,0 +1,49 @@
+#ifndef _SCHEDULER_
+#define _SCHEDULER_
+
+#include "../helper_classes/PQWrapper.h"
+#include "../helper_classes/Order.h"
+#include "../Behavior.h"
+
+#define NUM_TASKS 5
+#define WAITING_ROBOT 1
+#define NEW_ROBOT 2
+#define ORDERED_ROBOT 3
+
+typedef struct{
+  std::string name;
+  ros::Publisher topic;
+  int sched_status;
+} Robot;
+
+class Scheduler : Behavior {
+  std::vector<Robot> robots;
+	std::queue<Robot> waitingRobots;
+
+  PQWrapper* unassignedOrders;
+	std::vector<Order> assignedOrders;
+
+	void create_orders();
+
+	void waiting_dance();
+
+  void msg_callback(const std_msgs::String::ConstPtr& msg);
+
+public:
+  Scheduler(std::string scoutname, Sensors* sensors);
+	~Scheduler();
+	
+	void get_task(int robot);
+	
+	void task_complete(Order o);
+	void task_failed(Order o);
+	
+	
+	Order get_next_item();
+	
+	void run();
+
+  ros::Subscriber robot_to_sched;
+    
+};
+#endif
diff --git a/scout/libscout/src/test_behaviors/WH_Robot.cpp b/scout/libscout/src/test_behaviors/WH_Robot.cpp
new file mode 100644
index 0000000..17111ec
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/WH_Robot.cpp
@@ -0,0 +1,259 @@
+#include "WH_Robot.h"
+#include "../helper_classes/Order.h"
+
+/** @Brief: warehouse robot constructor **/
+WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors):
+            line_follow(scoutname, sensors) 
+{
+  nav_map = new navigationMap(scoutname, sensors);
+  curr_task = DEFAULT_TASK;
+  name = scoutname;
+  reg_failed = 1;
+  srand(0xDEADBEEF);
+}
+
+WH_Robot::~WH_Robot()
+{
+  delete(nav_map);
+}
+
+Duration WH_Robot::get_worst_case_time(State start_state, State target_state)
+{
+  return nav_map->get_worst_case_time(start_state, target_state);
+}
+
+/** @brief Drives from state 2 to the robot home base and goes to sleep for
+ *  x seconds
+ */
+void WH_Robot::go_home(int x)
+{
+  unsigned int curr_state = nav_map->get_state();
+  if(curr_state != 2)
+  {
+    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
+    return;
+  }
+  turn_straight();
+  follow_line();
+  turn_left();
+  follow_line();
+  Duration sleep_time(x);
+  sleep_time.sleep();
+  return;
+}
+
+void WH_Robot::leave_home()
+{
+  unsigned int curr_state = nav_map->get_state();
+  if(curr_state != 2)
+  {
+    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
+    return;
+  }
+  turn_straight();
+  follow_line();
+  nav_map->update_state(ISTRAIGHT);
+  turn_left();
+  follow_line();
+}
+
+/** @brief Based on the given path, make
+ *         the series of turns to follow this path,
+ *         updating the current state in the navigation map as we do so   
+ *  @param the Path to follow
+ */
+void WH_Robot::follow_path(Path path_to_follow){
+  for(int i=0; i<path_to_follow.len; i++)
+  {
+    Turn t = path_to_follow.path[i];
+    unsigned int curr_state = nav_map->get_state();
+
+    nav_map->update_state(t);
+    switch(t)
+    {
+      case ISTRAIGHT:
+        turn_straight();
+        follow_line();
+        if(curr_state == 2)
+        {
+          turn_straight();
+          follow_line();
+          turn_straight();
+          follow_line();
+        }
+        break;
+      case ILEFT:
+        turn_left();
+        follow_line();
+        break;
+      case IRIGHT:
+        if(9 <= curr_state && curr_state <= 12)
+        {
+          turn_straight();
+          follow_line();
+        }
+        turn_right();
+        follow_line();
+        if(curr_state <= 4)
+        {
+          turn_straight();
+          follow_line();
+        }
+        break;
+      case IUTURN:
+        u_turn();
+        follow_line();
+        break;
+      case ISPOTTURN:
+        spot_turn();
+        follow_line();
+        break;
+    }
+  }  
+}
+
+int WH_Robot::exec_task()
+{
+  assert(curr_task != DEFAULT_TASK);
+  
+  // remember where you started doing this task so we know where to return
+  // State home_state = nav_map->get_state();
+  // For now, use state 2.
+  Path new_path = nav_map->shortest_path(curr_task->get_dest());
+  curr_task->set_path(new_path);
+
+  follow_path(new_path);
+   
+  //TODO: forklift yaaaay
+
+  int did_task_complete;
+
+  int error = rand() % 12;
+  if(error < 9) //Fail with 1/4 probability
+  {
+    ROS_INFO("WH_robot: TASK COMPLETE!");
+    did_task_complete = TASK_COMPLETED;
+  }
+  else
+  {
+    ROS_INFO("WH_robot: TASK FAILED!");
+    did_task_complete = TASK_FAILED;
+  }
+  
+  // For now use state 2 as home state
+  // Path return_path = nav_map->shortest_path(home_state);
+  Path return_path = nav_map->shortest_path(2);
+  curr_task->set_path(return_path);
+  
+  follow_path(return_path);
+
+  return did_task_complete;
+}
+
+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 = (char*)msg->data.c_str();
+    char* data;
+    data = strtok(string, " ");
+
+    data = strtok(NULL, " ");
+    int order_id = atoi(data);
+
+    data = strtok(NULL, " ");
+    Address source = atoi(data);
+
+    data = strtok(NULL, " ");
+    Address dest = atoi(data);
+
+    Time start_time = ros::Time::now();
+    Path path;
+    path.len = 0;
+    path.path = NULL;
+    Duration est_time(0);
+
+    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
+  }
+  else
+  {
+    ROS_INFO("I got a bad message: %s", msg->data.c_str());
+  }
+}
+
+void WH_Robot::run ()
+{
+  ROS_INFO("A WH_Robot has been created");
+
+  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("I am a robot. Registering with scheduler...");
+  while(reg_failed && ok())
+  {
+    std_msgs::String msg;
+    std::stringstream ss;
+    ss << "REGISTER "<<name;
+    msg.data = ss.str();
+    robot_to_sched.publish(msg);
+
+    spinOnce();
+  }
+
+  follow_line();
+
+  while(ok())
+  {
+    ROS_INFO("WH_ROBOT: Running main loop");
+
+
+      std_msgs::String msg;
+      std::stringstream ss;
+      ss << "GET_TASK "<<id;
+      msg.data = ss.str();
+      robot_to_sched.publish(msg);
+
+    while(curr_task == DEFAULT_TASK && ok()){
+      spinOnce();
+    }
+
+    int error = exec_task();
+
+    /** Go to robot home base */
+    go_home(2);
+    leave_home();
+
+    if(error == TASK_COMPLETED)
+    {
+      std_msgs::String msg;
+      std::stringstream ss;
+      ss << "SUCCESS "<<curr_task->getid();
+      msg.data = ss.str();
+      robot_to_sched.publish(msg);
+    }
+    else //error == TASK_FAILED
+    {
+      std_msgs::String msg;
+      std::stringstream ss;
+      ss << "FAILED "<<curr_task->getid();
+      msg.data = ss.str();
+      robot_to_sched.publish(msg);
+    }
+    delete curr_task;
+    curr_task = DEFAULT_TASK;
+
+  }
+}
+
+void WH_Robot::set_task(Order order)
+{
+  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
+      order.get_start_time(), order.get_path(), order.get_est_time()); 
+  return;
+}
+
diff --git a/scout/libscout/src/test_behaviors/WH_Robot.h b/scout/libscout/src/test_behaviors/WH_Robot.h
new file mode 100644
index 0000000..21c4e89
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/WH_Robot.h
@@ -0,0 +1,44 @@
+#ifndef _WH_ROBOT_
+#define _WH_ROBOT_
+
+#define DEFAULT_TASK NULL
+#define TASK_COMPLETED 0
+#define TASK_FAILED -1
+
+#include "../behaviors/line_follow.h"
+#include "../Behavior.h"
+#include "../behaviors/navigationMap.h"
+#include "../helper_classes/Order.h"
+#include <assert.h>
+#include <stdlib.h>
+
+class WH_Robot : line_follow{
+        std::string name;
+        int id;
+
+        int reg_failed;
+
+        Order* curr_task;
+        navigationMap* nav_map;
+
+        Duration get_worst_case_time(State start_state, State target_state);
+        int exec_task();
+
+        void robot_callback(const std_msgs::String::ConstPtr& msg);
+
+        void go_home(int x);
+        void leave_home();
+
+    public:
+        WH_Robot(std::string scoutname, Sensors* sensors);
+        ~WH_Robot();
+        void run();
+
+        void set_task(Order order);
+        void follow_path(Path path_to_follow);
+
+        ros::Publisher robot_to_sched;
+        ros::Subscriber sched_to_robot;
+};
+
+#endif
diff --git a/scout/libscout/src/test_behaviors/maze_solve.cpp b/scout/libscout/src/test_behaviors/maze_solve.cpp
new file mode 100644
index 0000000..b5e272b
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/maze_solve.cpp
@@ -0,0 +1,279 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "maze_solve.h"
+
+using namespace std;
+
+
+// want to have a minimal working thing, use a big enough 
+// static array and start in the middle
+// we assume we are facing right, that affects where we store
+// wall information
+// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical
+#define WALL -1
+#define UNSEEN 0
+#define SEEN 1
+#define CRITICAL 2
+// facings
+#define UP 0
+#define RIGHT 1
+#define DOWN 2
+#define LEFT 3
+
+Duration sonar_update_time(1.5);
+
+void maze_solve::run(){
+    
+    // TODO:first initialize map to all 0's
+    ROS_INFO("Starting to solve the maze");
+    // Go up to the first line.
+    follow_line();
+    // Turn the sonar on.
+    sonar->set_on();
+    sonar->set_range(0, 23);
+
+    // Wait for the sonar to initialize.
+    while(!look_around(25, 25, RIGHT) && ok())
+    {
+      spinOnce();      
+    }
+
+    // Solve the maze
+    bool finished = solve(25,25, RIGHT);
+
+    // Check and report final condition.
+    if (finished)
+        ROS_INFO("YAY! I have solved the maze");
+    else
+        ROS_INFO("NO! The maze is unsolvable");
+}
+
+bool maze_solve::solve(int row, int col, int dir)
+{
+    int initial_dir = dir;
+
+    ROS_INFO("I am at direction %d", dir);
+
+    // use backtracking to solve the maze
+    if (at_destination())
+        return true;
+
+    // Wait for sonar to update.
+    sonar_update_time.sleep();
+
+    // this function should fill the adjacent cells around me with
+    // wall's or paths
+    while(!look_around(row, col, dir) && ok())
+    {
+        spinOnce();
+    }
+
+    /* try go up */
+    if (map[row-1][col] != WALL && initial_dir != UP)
+    {
+    ROS_INFO("GOING UP!");
+        // Turn up.
+        turn_from_to(dir, UP);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row-1, col, DOWN);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = UP;
+        }
+    }
+    /* try right */
+    if (map[row][col+1] != WALL && initial_dir != RIGHT)
+    {
+    ROS_INFO("GOING RIGHT!");
+        // Turn right.
+        turn_from_to(dir, RIGHT);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row, col+1, LEFT);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = RIGHT;
+        }
+    }
+    /* try down */
+    if (map[row+1][col] != WALL && initial_dir != DOWN)
+    {
+    ROS_INFO("GOING DOWN!");
+        // Turn down.
+        turn_from_to(dir, DOWN);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row+1, col, UP);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = DOWN;
+        }
+    }
+    /* try left */
+    if (map[row][col-1] != WALL && initial_dir != LEFT)
+    {
+    ROS_INFO("GOING LEFT!");
+        // Turn down.
+        turn_from_to(dir, LEFT);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row, col-1, RIGHT);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = LEFT;
+        }
+    }
+
+    ROS_INFO("DEAD END FOUND, TURNING BACK.");
+    // we have exhausted all the options. This path is clearly a
+    // dead end. go back to where we come from and return false.
+    turn_from_to(dir, initial_dir);
+    follow_line();
+    return false;
+}
+
+// this function takes in the current direction and turns the scout
+// into it intended direction
+void maze_solve::turn_from_to(int current_dir, int intended_dir) {
+    switch ((4 + intended_dir - current_dir) % 4) 
+    {
+        case 0:
+            spot_turn();
+            break;
+        case 1:
+            turn_left();
+            break;
+        case 2:
+            turn_straight();
+            break;
+        case 3:
+            turn_right();
+            break;
+    }
+}
+
+bool maze_solve::look_around(int row, int col, int dir)
+{
+    // look around current place using sonar
+    // store whether or not
+    // there is a wall into the map
+    // stores at row col 2 if point is critical, 1 otherwise
+    
+    int* readings = sonar->get_sonar_readings();
+    spinOnce();
+
+/*
+    // Look to the left.
+    int left_distance = (readings[1] + readings[0] + readings[47])/3;
+    // Look to the front.
+    int front_distance = (readings[35] + readings[36] + readings[37])/3;
+    // Look to the right.
+    int right_distance = (readings[23] + readings[24] + readings[25])/3;
+*/
+    // Look to the left.
+    int left_distance = readings[0];
+    // Look to the front.
+    int front_distance = readings[36];
+    // Look to the right.
+    int right_distance = readings[24];
+
+    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);
+
+    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
+      return false;
+
+    switch (dir)
+    {
+        case UP:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row][col+1] = (left_distance < 500)?WALL:SEEN;
+            map[row+1][col] = (front_distance < 500)?WALL:SEEN;
+            map[row][col-1] = (right_distance < 500)?WALL:SEEN;
+            break;
+        case RIGHT:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row+1][col] = (left_distance < 500)?WALL:SEEN;
+            map[row][col-1] = (front_distance < 500)?WALL:SEEN;
+            map[row-1][col] = (right_distance < 500)?WALL:SEEN;
+            break;
+        case DOWN:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row][col-1] = (left_distance < 500)?WALL:SEEN;
+            map[row-1][col] = (front_distance < 500)?WALL:SEEN;
+            map[row][col+1] = (right_distance < 500)?WALL:SEEN;
+            break;
+        case LEFT:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row-1][col] = (left_distance < 500)?WALL:SEEN;
+            map[row][col+1] = (front_distance < 500)?WALL:SEEN;
+            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
+            break;
+    }
+
+    return true;
+}
+
+bool maze_solve::at_destination() 
+{
+    vector<uint32_t> readings = linesensor->query();
+    if ( readings[0] > 200 &&
+         readings[1] < 55 &&
+         readings[2] < 55 &&
+         readings[3] > 200 &&
+         readings[4] > 200 &&
+         readings[5] < 55 &&
+         readings[6] < 55 &&
+         readings[7] > 200 )
+    {
+        return true;
+    }
+    return false;
+}
diff --git a/scout/libscout/src/test_behaviors/maze_solve.h b/scout/libscout/src/test_behaviors/maze_solve.h
new file mode 100644
index 0000000..ae590df
--- /dev/null
+++ b/scout/libscout/src/test_behaviors/maze_solve.h
@@ -0,0 +1,46 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _MAZE_SOLVE_H_
+#define _MAZE_SOLVE_H_
+
+#include "../behaviors/line_follow.h"
+
+class maze_solve: public line_follow
+{
+    public:
+        maze_solve(std::string scoutname, Sensors* sensors):
+            line_follow(scoutname, "maze_solve", sensors) {};
+        void run();
+    private:
+        bool solve(int row, int col, int dir);
+        void turn_from_to(int current_dir, int intended_dir);
+        bool look_around(int row, int col, int dir);
+        bool at_destination();
+
+        int map[60][60];
+};
+#endif
+
diff --git a/scout/scoutsim/GUI.py b/scout/scoutsim/GUI.py
index ab425f8..4e11ee9 100755
--- a/scout/scoutsim/GUI.py
+++ b/scout/scoutsim/GUI.py
@@ -19,6 +19,7 @@ import signal
 # ros imports for spawning and killing scout
 import rospy
 import roslib
+import os
 roslib.load_manifest("scoutsim")
 
 # scoutsim imports
@@ -36,21 +37,40 @@ class Behaviors(object):
                           "Navigation Map", "Scheduler", "Warehouse",
                           "Line Follow", "WL Test", "Maze Solve"]
     @classmethod
+    def getListFromDir(self):
+        behaviors = []
+        for path, dirname, fnames in os.walk('../libscout/src'):
+            if 'src/behaviors' in path or 'src/test_behaviors' in path:
+                path_parts = path.split('/')
+
+                for f in sorted(fnames):
+                    # The pause_scout behavior needs to go first!
+                    if f.endswith('.h') and 'pause_scout' in f:
+                        behaviors.insert(0, f.split('.')[0])
+                    # Everything else goes in alphabetical order
+                    elif f.endswith('.h'):
+                        behaviors.append(f.split('.')[0])
+        return behaviors
+
+    @classmethod
     def getNumber(self, behaviorName):
-        if (behaviorName in Behaviors.behaviors):
-            return Behaviors.behaviors.index(behaviorName)
+        behaviors = self.getListFromDir()
+        if (behaviorName in behaviors):
+            return behaviors.index(behaviorName)
     
     @classmethod
     def getName(self, index):
+        behaviors = self.getListFromDir()
         if (0 <= index < len(self.Behavior)):
-            return Behaviors.behaviors[index]
+            return behaviors[index]
         else:
             return -1
     
     @classmethod
     def getBehaviors(self):
         # "Pause" is not considered a behavior in GUI
-        return Behaviors.behaviors[1:]
+        behaviors = self.getListFromDir()
+        return behaviors[1:]
 
 # each scout is represented by this class
 class Scout(object):
