commit 5d0687a9991d806c94fc7c21405334e6717e3611
Author: Priya <pdeo@Phlegethon.(none)>
Date:   Tue Apr 24 16:22:35 2012 -0400

    Small changes to wireless, and starting turning with line following.

diff --git a/scout/libscout/CMakeLists.txt b/scout/libscout/CMakeLists.txt
index 410bccd..cd53452 100644
--- a/scout/libscout/CMakeLists.txt
+++ b/scout/libscout/CMakeLists.txt
@@ -30,10 +30,10 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #target_link_libraries(example ${PROJECT_NAME})
 
 set(MAIN_FILES 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/sim_line.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/sim_line.cpp src/behaviors/wl_test.cpp)
 set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.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/EncodersControl.cpp src/LinesensorControl.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)
 
 rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES} ${CONTROL_CLASSES} ${HELPER_FILES})
 rosbuild_add_executable(test_encoders src/test_encoders.cpp)
diff --git a/scout/libscout/src/Behavior.cpp b/scout/libscout/src/Behavior.cpp
index b89a796..f76872c 100644
--- a/scout/libscout/src/Behavior.cpp
+++ b/scout/libscout/src/Behavior.cpp
@@ -58,6 +58,8 @@ Behavior::Behavior(string scoutname, string my_name)
     //cliffsensor = new CliffsensorControl(node, scoutname);
     encoders = new EncodersControl(node, scoutname);
     linesensor = new LinesensorControl(node, scoutname);
+    wl_sender = new WirelessSender(node);
+    wl_receiver = new WirelessReceiver(node);
 
     loop_rate = new ros::Rate(10);
 }
diff --git a/scout/libscout/src/Behavior.h b/scout/libscout/src/Behavior.h
index a665699..8dd983f 100644
--- a/scout/libscout/src/Behavior.h
+++ b/scout/libscout/src/Behavior.h
@@ -47,6 +47,8 @@
 //#include "CliffsensorControl.h"
 #include "EncodersControl.h"
 #include "LinesensorControl.h"
+#include "WirelessSender.h"
+#include "WirelessReceiver.h"
 #include "constants.h"
 
 typedef ros::Time Time;
@@ -77,6 +79,8 @@ class Behavior
         //CliffsensorControl * cliffsensor;
         EncodersControl * encoders;
         LinesensorControl * linesensor;
+        WirelessSender * wl_sender;
+        WirelessReceiver * wl_receiver;
 
         // Wrappers for ROS functions
         bool ok();
diff --git a/scout/libscout/src/BehaviorList.cpp b/scout/libscout/src/BehaviorList.cpp
index 6f0a56f..926c456 100644
--- a/scout/libscout/src/BehaviorList.cpp
+++ b/scout/libscout/src/BehaviorList.cpp
@@ -9,6 +9,7 @@ BehaviorList::BehaviorList(std::string scoutname)
   behavior_list.push_back((Behavior*)new Scheduler(scoutname));
   behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
   behavior_list.push_back((Behavior*)new sim_line(scoutname));
+  behavior_list.push_back((Behavior*)new wl_test(scoutname));
   return;
 }
 
diff --git a/scout/libscout/src/BehaviorList.h b/scout/libscout/src/BehaviorList.h
index a667717..5a4c111 100644
--- a/scout/libscout/src/BehaviorList.h
+++ b/scout/libscout/src/BehaviorList.h
@@ -9,6 +9,7 @@
 #include "behaviors/navigationMap.h"
 #include "behaviors/Scheduler.h"
 #include "behaviors/WH_Robot.h"
+#include "behaviors/wl_test.h"
 
 class BehaviorList
 {
diff --git a/scout/libscout/src/LinesensorControl.cpp b/scout/libscout/src/LinesensorControl.cpp
index ba15ec1..c36d780 100644
--- a/scout/libscout/src/LinesensorControl.cpp
+++ b/scout/libscout/src/LinesensorControl.cpp
@@ -35,6 +35,8 @@
 
 using namespace std;
 
+static double last_ret;
+
 /**
  * @brief Initialize the line following module of libscout.
  */
@@ -62,3 +64,43 @@ vector<uint32_t> LinesensorControl::query()
     return srv.response.readings;
 }
 
+/** 
+ *  @brief Returns the average readings of the line following sensors. 
+ */
+double LinesensorControl::readline()
+{
+    unsigned int total_read = 0;
+    unsigned int weighted_total = 0;
+
+    vector<uint32_t> readings = query();
+
+    for (int i = 0; i < 8; i++)
+    {
+        total_read += readings[i];
+        weighted_total += i * readings[i];
+    }
+
+    if (total_read == 0)
+    {
+        return last_ret;
+    }
+    
+    double ret_val = last_ret = ((double) weighted_total / total_read) - 4;
+    return ret_val;
+}
+
+bool LinesensorControl::fullline()
+{
+    return false; 
+}
+
+void LinesensorControl::turnLeft()
+{
+    vector<uint32_t> readings = query();
+}
+
+void LinesensorControl::turnRight()
+{
+    vector<uint32_t> readings = query();
+
+}
diff --git a/scout/libscout/src/LinesensorControl.h b/scout/libscout/src/LinesensorControl.h
index c5a3e5a..8cf41ca 100644
--- a/scout/libscout/src/LinesensorControl.h
+++ b/scout/libscout/src/LinesensorControl.h
@@ -49,10 +49,20 @@ class LinesensorControl
         /** Use ROS to get the current readings. */
         std::vector<uint32_t> query();
 
+        /** Get line position */
+        double readline();
+
+        /** Implement turn functions */
+        void turnLeft();
+        void turnRight();
+
     private:
         /* ROS publisher and client declaration */
         ros::ServiceClient query_client;
         ros::NodeHandle node;
+
+        /** Check if we are at an intersecetion */
+        bool fullline();
 };
 
 #endif /* _LINESENSOR_CONTORL_H_ */
diff --git a/scout/libscout/src/WirelessReceiver.cpp b/scout/libscout/src/WirelessReceiver.cpp
index 73b8309..cf25d69 100644
--- a/scout/libscout/src/WirelessReceiver.cpp
+++ b/scout/libscout/src/WirelessReceiver.cpp
@@ -42,15 +42,25 @@ using namespace std;
 /**
  * @brief Initializes wireless receiver
  * 
+ * @param libscout_node Node handle of the ROS node used by libscout.
+ **/
+WirelessReceiver::WirelessReceiver(const ros::NodeHandle& libscout_node)
+    : node(libscout_node)
+{
+    sub = node.subscribe("/wireless/receive", QUEUE_SIZE,
+          &WirelessReceiver::receive, this);
+}
+
+/**
+ * @brief Registers a function to be called upon receipt of a packet.
+ *
  * @param callback A function pointer to a function which is called whenever a
  * wireless packet is received. It should be of form:
  *     void callback(const WirelessPacketConstPtr& packet);
  **/
-WirelessReceiver::WirelessReceiver(WirelessCallback callback)
-    : callback(callback)
+void WirelessReceiver::register_callback(WirelessCallback new_callback)
 {
-    sub = nh.subscribe("/wireless/receive", QUEUE_SIZE,
-        &WirelessReceiver::receive, this);
+    callback = new_callback;
 }
 
 /**
@@ -60,5 +70,12 @@ WirelessReceiver::WirelessReceiver(WirelessCallback callback)
  **/
 void WirelessReceiver::receive(const ::messages::WirelessPacketConstPtr& packet)
 {
-    (*callback)(packet);
+    if (callback == NULL)
+    {
+        ROS_WARN("Warning: Ignored wireless packet.");
+    }
+    else
+    {
+        (*callback)(packet->data);
+    }
 }
diff --git a/scout/libscout/src/WirelessReceiver.h b/scout/libscout/src/WirelessReceiver.h
index 7eba7f0..7227871 100644
--- a/scout/libscout/src/WirelessReceiver.h
+++ b/scout/libscout/src/WirelessReceiver.h
@@ -43,17 +43,22 @@
 #include "messages/WirelessPacket.h"
 #include "constants.h"
 
-typedef void (*WirelessCallback)(const ::messages::WirelessPacketConstPtr&);
+typedef void (*WirelessCallback)(std::vector<uint8_t>);
 
 class WirelessReceiver
 {
-public:
-    WirelessReceiver(WirelessCallback callback);
-private:
-    ros::NodeHandle nh;
-    ros::Subscriber sub;
-    WirelessCallback callback;
-    void receive(const ::messages::WirelessPacketConstPtr&);
+    public:
+        /** Set up the class and prepare to communicate over ROS */
+        WirelessReceiver(const ros::NodeHandle& libscout_node);
+
+        /** Register a callback to be called every time a packet is received */
+        void register_callback(WirelessCallback new_callback);
+
+    private:
+        ros::Subscriber sub;
+        ros::NodeHandle node;
+        WirelessCallback callback;
+        void receive(const ::messages::WirelessPacketConstPtr&);
 };
 
 #endif
diff --git a/scout/libscout/src/WirelessSender.cpp b/scout/libscout/src/WirelessSender.cpp
index 9de7d5b..2ca366e 100644
--- a/scout/libscout/src/WirelessSender.cpp
+++ b/scout/libscout/src/WirelessSender.cpp
@@ -40,25 +40,21 @@
 using namespace std;
 using namespace ::messages;
 
-WirelessSender::WirelessSender(uint16_t type, uint16_t dest, uint16_t pan,
-    uint16_t channel)
+WirelessSender::WirelessSender(const ros::NodeHandle& libscout_node,
+                               uint16_t type, uint16_t pan, uint16_t channel)
+    : node(libscout_node)
 {
     packet.type = type;
-    packet.dest = dest;
     packet.pan = pan;
     packet.channel = channel;
-    pub = nh.advertise< ::messages::WirelessPacket>("/wireless/send",
-        QUEUE_SIZE);
+    pub = node.advertise< ::messages::WirelessPacket>("/wireless/send",
+                                                      QUEUE_SIZE);
 }
 
 void WirelessSender::setType(uint16_t type)
 {
     packet.type = type;
 }
-void WirelessSender::setDest(uint16_t dest)
-{
-    packet.dest = dest;
-}
 void WirelessSender::setPan(uint16_t pan)
 {
     packet.pan = pan;
@@ -68,23 +64,20 @@ void WirelessSender::setChannel(uint16_t channel)
     packet.channel = channel;
 }
 
-void WirelessSender::send(uint8_t data[], int length)
+void WirelessSender::send(uint8_t data[], int length, uint16_t dest)
 {
+    packet.dest = dest;
     packet.data.clear();
     for (int i = 0; i < length; i++)
     {
         packet.data.push_back(data[i]);
     }
-    publish();
+    pub.publish(packet);
 }
 
-void WirelessSender::send(vector<uint8_t>& data)
+void WirelessSender::send(vector<uint8_t>& data, uint16_t dest)
 {
+    packet.dest = dest;
     packet.data = data;
-    publish();
-}
-
-void WirelessSender::publish()
-{
     pub.publish(packet);
 }
diff --git a/scout/libscout/src/WirelessSender.h b/scout/libscout/src/WirelessSender.h
index d515462..7ab72bf 100644
--- a/scout/libscout/src/WirelessSender.h
+++ b/scout/libscout/src/WirelessSender.h
@@ -31,6 +31,10 @@
  * Contains functions and definitions for the use of
  * XBee wireless
  *
+ * Uses the dest field of the wireless packet to specify which
+ * scout the message is intended for, or SCOUT_ALL to send to
+ * all scouts
+ *
  * @author Colony Project, CMU Robotics Club
  * @author Tom Mullins
  **/
@@ -43,28 +47,28 @@
 #include "messages/WirelessPacket.h"
 #include "constants.h"
 
+/// A macro to allow sending to all the scouts.
+#define SCOUT_ALL (messages::WirelessPacket::DEST_BROADCAST)
+
 class WirelessSender
 {
     public:
-        WirelessSender(uint16_t type = 0,
-                uint16_t dest = messages::WirelessPacket::DEST_BROADCAST,
+        WirelessSender(const ros::NodeHandle& libscout_node,
+                uint16_t type = 0,
                 uint16_t pan = messages::WirelessPacket::PAN_DEFAULT,
                 uint16_t channel = messages::WirelessPacket::CHANNEL_DEFAULT);
         
         void setType(uint16_t type);
-        void setDest(uint16_t dest);
         void setPan(uint16_t pan);
         void setChannel(uint16_t channel);
         
-        void send(uint8_t data[], int length);
-        void send(std::vector<uint8_t>& data);
+        void send(uint8_t data[], int length, uint16_t dest = SCOUT_ALL);
+        void send(std::vector<uint8_t>& data, uint16_t dest = SCOUT_ALL);
         
     private:
-        ros::NodeHandle nh;
+        ros::NodeHandle node;
         ros::Publisher pub;
         messages::WirelessPacket packet;
-        
-        void publish();
 };
 
 #endif
diff --git a/scout/libscout/src/behaviors/Scheduler.cpp b/scout/libscout/src/behaviors/Scheduler.cpp
index 3b5c0d3..65248bf 100644
--- a/scout/libscout/src/behaviors/Scheduler.cpp
+++ b/scout/libscout/src/behaviors/Scheduler.cpp
@@ -45,9 +45,12 @@ void Scheduler::create_orders()
   p.path = NULL;
   Time t = ros::Time::now();
   Duration end(0);
-	Order a(1,0,1,t,p,end);
-  Order b(2,0,1,t,p,end);
-  Order c(3,0,9,t,p,end);
+  Duration five(5);
+  Duration three(3);
+  Duration ten(10);
+	Order a(1,0,1,t+ten,p,end);
+  Order b(2,0,1,t+five,p,end);
+  Order c(3,0,9,t+three,p,end);
   Order d(4,0,2,t,p,end);
   Order e(5,0,4,t,p,end);
 	
@@ -94,6 +97,7 @@ Order Scheduler::get_next_item()
   double time = t.toSec();
 
   Order* best;
+  int best_index;
   for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
   {
     Order order = unassignedOrders->peek(i);
@@ -103,10 +107,15 @@ Order Scheduler::get_next_item()
     if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
     {
       best = &order;
+      best_index = i;
       time = end_time.toSec() + MAX_WAIT_TIME;
     }
   }
-  return *best;
+
+  Order ret;
+  ret = unassignedOrders->remove(best_index);
+  assignedOrders.push_back(ret);
+  return ret;
 }
 
 void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
@@ -137,6 +146,7 @@ void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
   }
   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);
   }
@@ -164,7 +174,7 @@ void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
     ss<<"REG_SUCCESS "<<id;
     msg.data = ss.str();
     new_robot.topic.publish(msg);
-    ros::spinOnce();
+    spinOnce();
 
     ROS_INFO("Registration a success");
 
@@ -194,8 +204,11 @@ void Scheduler::run()
 	{
     ROS_INFO("Scheduler running");
     spinOnce();
-		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
+		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
+    {
       waiting_dance();
+      spinOnce();
+    }
   
     ROS_INFO("SCHEDULER: Setting task");
     Order next = get_next_item();
@@ -207,6 +220,8 @@ void Scheduler::run()
     ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
     msg.data = ss.str();
     r.topic.publish(msg);
+  
+    spinOnce();
 
     waitingRobots.front().sched_status = ORDERED_ROBOT;
 		waitingRobots.pop();
diff --git a/scout/libscout/src/behaviors/WH_Robot.cpp b/scout/libscout/src/behaviors/WH_Robot.cpp
index fcff9b2..acd7cba 100644
--- a/scout/libscout/src/behaviors/WH_Robot.cpp
+++ b/scout/libscout/src/behaviors/WH_Robot.cpp
@@ -24,10 +24,22 @@ int WH_Robot::exec_task()
 {
   ROS_INFO("WH_robot: Executing a task");
   assert(curr_task != DEFAULT_TASK);
-  //TODO: do task
+  
+  Path new_path = nav_map->shortest_path(curr_task->get_dest());
+  curr_task->set_path(new_path);
+  for(int i=0; i<new_path.len; i++)
+  {
+    Turn t = new_path.path[i];
+    ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
+    nav_map->update_state(t);
+    Duration time = nav_map->get_time_remaining();
+    while(time.sec > 0)
+      time = nav_map->get_time_remaining();
+  }
+  ROS_INFO("Path complete at state %d", (int)nav_map->get_state());
   srand(0xDEADBEEF);
   int error = rand() % 12;
-  if(error < 9) //Fail with 1/4 probability
+  if(error < 6) //Fail with 1/2 probability
   {
     ROS_INFO("WH_robot: TASK COMPLETE!");
     return TASK_COMPLETED;
diff --git a/scout/libscout/src/behaviors/sim_line.cpp b/scout/libscout/src/behaviors/sim_line.cpp
index 123bc7b..9ab3226 100644
--- a/scout/libscout/src/behaviors/sim_line.cpp
+++ b/scout/libscout/src/behaviors/sim_line.cpp
@@ -31,7 +31,6 @@ static int motor_l;
 static int motor_r;
 static double last_ret;
 
-
 /** TODO: This should be part of the line sensor library */
 double sim_line::get_line_pos(vector<uint32_t> readings)
 {
@@ -60,7 +59,7 @@ void sim_line::run()
 
     while(ok())
     {
-        double line_loc = get_line_pos(linesensor->query());
+        double line_loc = linesensor->readline();
 
         cout << "line_loc: " << line_loc << endl;
 
