commit af7e0f941c6ec3ce22eb88a8e49af36147359107
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Tue Feb 19 18:18:39 2013 -0500

    Added new at_destination functionality.
    
    Maze turns are buggy and are the next order of business.

diff --git a/scout/libscout/src/LinesensorControl.cpp b/scout/libscout/src/LinesensorControl.cpp
index b7176a7..8d2bc2b 100644
--- a/scout/libscout/src/LinesensorControl.cpp
+++ b/scout/libscout/src/LinesensorControl.cpp
@@ -61,6 +61,12 @@ vector<uint32_t> LinesensorControl::query()
         ROS_ERROR("LinesensorControl query failed.");
     }
 
+    if (srv.response.readings.size() != 8)
+    {
+        ROS_WARN("Linesensor reading vector has %d readings, 8 expected.",
+                 int(srv.response.readings.size()));
+    }
+
     return srv.response.readings;
 }
 
@@ -94,9 +100,13 @@ double LinesensorControl::readline()
     return ret_val;
 }
 
-bool LinesensorControl::fullline()
+bool LinesensorControl::fullline(vector<uint32_t> readings)
 {
-    vector<uint32_t> readings = query();
+    if (readings.size() < 8)
+    {
+        return false;
+    }
+
     for(int i=0; i<8; i++)
     {
         if(readings[i] < 200)
@@ -105,3 +115,41 @@ bool LinesensorControl::fullline()
     return true; 
 }
 
+/**
+ * We define a destination as two gaps of white between the line and patches
+ * of 15-shade color (light grey) on either side.
+ * I use 15-shade color in an attempt to not throw off the line localization.
+ */
+bool LinesensorControl::destination(vector<uint32_t> readings)
+{
+    if (readings.size() < 8)
+    {
+        return false;
+    }
+
+    ROS_INFO("Destination call. Readings: %d %d %d %d %d %d %d %d",
+            readings[0], readings[1], readings[2], readings[3],
+            readings[4], readings[5], readings[6], readings[7]);
+
+    // Try to match this pattern to what we see
+    int expected_pattern[] = {15, 0, 255, 0, 15};
+    int pat_idx = 0;
+
+    for (int i = 0; i < 8; i++)
+    {
+        // If the difference between what we have and expect is less than 5
+        if (abs(readings[i] - expected_pattern[pat_idx]) < 5)
+        {
+            // Check. Keep looking for the rest of the pattern.
+            pat_idx++;
+        }
+
+        // The whole pattern was found
+        if (pat_idx > 4)
+        {
+            ROS_INFO("FOUND DESTINATION");
+            return true;
+        }
+    }
+    return false;
+}
diff --git a/scout/libscout/src/LinesensorControl.h b/scout/libscout/src/LinesensorControl.h
index 3b64fc4..9138581 100644
--- a/scout/libscout/src/LinesensorControl.h
+++ b/scout/libscout/src/LinesensorControl.h
@@ -53,7 +53,11 @@ class LinesensorControl
         double readline();
 
         /** Check if we are at an intersecetion */
-        bool fullline();
+        bool fullline(std::vector<uint32_t> readings);
+
+        /** Check if we are at the destination for a maze
+         *  (a special line pattern) */
+        bool destination(std::vector<uint32_t> readings);
 
     private:
         /* ROS publisher and client declaration */
diff --git a/scout/libscout/src/behaviors/line_follow.cpp b/scout/libscout/src/behaviors/line_follow.cpp
index 66ddf9b..8dcfb0c 100644
--- a/scout/libscout/src/behaviors/line_follow.cpp
+++ b/scout/libscout/src/behaviors/line_follow.cpp
@@ -34,9 +34,11 @@ Duration init_turn_time(0.4);
 
 void line_follow::follow_line()
 {
+    vector<uint32_t> readings;
     do
     {
         double line_loc = linesensor->readline();
+        ROS_INFO("Line loc: %lf", line_loc);
 
         if (line_loc == 0.0)
         {
@@ -48,21 +50,28 @@ void line_follow::follow_line()
         motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127);
 
         motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
+        readings = linesensor->query();
     }
-    while(!linesensor->fullline());
+    while(!linesensor->fullline(readings) &&
+          !linesensor->destination(readings) &&
+          ok());
     halt();
 }
 
 void line_follow::turn_straight()
 {
+  vector<uint32_t> readings;
   do
   {
     motor_l = -MOTOR_BASE;
     motor_r = -MOTOR_BASE;
 
     motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
+    readings = linesensor->query();
   }
-  while(linesensor->fullline());
+  while(!linesensor->fullline(readings) &&
+        !linesensor->destination(readings) &&
+        ok());
 }
 
 void line_follow::turn_left()
@@ -84,7 +93,7 @@ void line_follow::turn_left()
 
     line_loc = linesensor->readline();
   }
-  while(!(-1 < line_loc && line_loc < 1));
+  while(!(-1 < line_loc && line_loc < 1) && ok());
 }
 
 void line_follow::halt()
@@ -109,13 +118,13 @@ void line_follow::spot_turn()
         {
           line_loc = linesensor->readline();
         }
-        while(line_loc < 2 && line_loc > -2);
+        while(line_loc < 2 && line_loc > -2 && ok());
 
         first = false;
     }
     line_loc = linesensor->readline();
   }
-  while(!(-1 < line_loc && line_loc < 1));
+  while(!(-1 < line_loc && line_loc < 1) && ok());
 }
 
 void line_follow::turn_right()
@@ -137,7 +146,7 @@ void line_follow::turn_right()
 
     line_loc = linesensor->readline();
   }
-  while(!(-1 < line_loc && line_loc < 1));
+  while(!(-1 < line_loc && line_loc < 1) && ok());
 }
 
 void line_follow::u_turn()
diff --git a/scout/scoutsim/maps/maze1.bmp b/scout/scoutsim/maps/maze1.bmp
index aac2445..b9015cd 100644
Binary files a/scout/scoutsim/maps/maze1.bmp and b/scout/scoutsim/maps/maze1.bmp differ
diff --git a/scout/scoutsim/maps/maze1_lines.bmp b/scout/scoutsim/maps/maze1_lines.bmp
index 1469679..2116ca4 100644
Binary files a/scout/scoutsim/maps/maze1_lines.bmp and b/scout/scoutsim/maps/maze1_lines.bmp differ
