commit ade1b7f9e6aee15cde7890ca6330f9cd57e118b5
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Tue Apr 10 16:53:00 2012 -0400

    Basic line sensor readings!

diff --git a/scout/libscout/src/LinesensorControl.cpp b/scout/libscout/src/LinesensorControl.cpp
index cd92312..8397df2 100644
--- a/scout/libscout/src/LinesensorControl.cpp
+++ b/scout/libscout/src/LinesensorControl.cpp
@@ -64,7 +64,9 @@ void LinesensorControl::query()
 
     for (int i = 0; i < 8; i++)
     {
-        cout << "[" << i << "]: " << srv.response.readings[i] << ", " << endl;
+        cout << "[" << i << "]: " << srv.response.readings[i] << ", ";
     }
+
+    cout << endl;
 }
 
diff --git a/scout/libscout/src/behaviors/sim_line.cpp b/scout/libscout/src/behaviors/sim_line.cpp
index b50f2f2..51653aa 100644
--- a/scout/libscout/src/behaviors/sim_line.cpp
+++ b/scout/libscout/src/behaviors/sim_line.cpp
@@ -29,7 +29,7 @@ void sim_line::run()
 {
     while(ok())
     {
-        motors->set_sides(40, 80, MOTOR_ABSOLUTE);
+        motors->set_sides(10, 20, MOTOR_ABSOLUTE);
 
         linesensor->query();
 
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 4fd0e11..c6c7a1b 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -96,6 +96,11 @@ namespace scoutsim
                                   &Scout::query_linesensor_callback,
                                   this);
 
+        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
+        {
+            linesensor_readings.push_back(0);
+        }
+
         meter = scout.GetHeight();
     }
 
@@ -166,22 +171,59 @@ namespace scoutsim
     bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
                                           linesensor::query_linesensor::Response& res)
     {
-        vector<unsigned int> readings;
+        res.readings = linesensor_readings;
+
+        return true;
+    }
 
-        for (unsigned int i = 0; i < 8; i++)
+    // Scale to linesensor value
+    unsigned int Scout::rgb_to_linesensor_val(unsigned char r,
+                                              unsigned char g,
+                                              unsigned char b)
+    {
+        // Should be 0 to 255
+        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
+
+        /// @todo Convert to the proper range
+        return 255 - grey;
+    }
+
+    void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta)
+    {
+        linesensor_readings.clear();
+
+        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
+        for (int s = 0; s < NUM_LINESENSORS; s++)
         {
-            readings.push_back(8 - i);
-        }
+            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
 
-        res.readings = readings;
+            int sensor_x = (int) (x - LNSNSR_D * cos(theta) - offset * sin(theta));
+            int sensor_y = (int) (y + LNSNSR_D * sin(theta) - offset * cos(theta));
 
-        return true;
+            /*
+            if (s == 0)
+            {
+                cout << "s: " << s << ", offset: " << offset << ", scout: (" << x <<
+                        ", " << y << "), sensor: (" << sensor_x <<
+                        ", " << sensor_y << ")" << endl;
+            }
+            */
+
+            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
+            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
+            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
+
+            unsigned int reading = rgb_to_linesensor_val(r, g, b);
+
+            linesensor_readings.push_back(reading);
+        }
     }
 
     /// Sends back the position of this scout so scoutsim can save
     /// the world state
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
                                         const wxImage& path_image,
+                                        const wxImage& lines_image,
                                         wxColour background_color,
                                         world_state state)
     {
@@ -225,7 +267,7 @@ namespace scoutsim
             wxImage rotated_image =
                 scout_image.Rotate(orient - PI/2.0,
                                    wxPoint(scout_image.GetWidth() / 2,
-                                            scout_image.GetHeight() / 2),
+                                           scout_image.GetHeight() / 2),
                                    false);
 
             for (int y = 0; y < rotated_image.GetHeight(); ++y)
@@ -252,6 +294,8 @@ namespace scoutsim
         p.angular_velocity = ang_vel;
         pose_pub.publish(p);
 
+        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
+
         // Figure out (and publish) the color underneath the scout
         {
             wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index a59410f..200f2be 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -73,6 +73,11 @@
 /// The scale factor so the speed of scout robots is reasonable for the sim
 #define SPEED_SCALE_FACTOR 0.05
 
+#define NUM_LINESENSORS 8
+
+// Distance, pixels, from center of robot to the linesensors.
+#define LNSNSR_D 20
+
 namespace scoutsim
 {
     struct Vector2
@@ -109,6 +114,7 @@ namespace scoutsim
 
             geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
                                          const wxImage& path_image,
+                                         const wxImage& lines_image,
                                          wxColour background_color,
                                          world_state state);
             void paint(wxDC& dc);
@@ -122,6 +128,10 @@ namespace scoutsim
                                          encoders::query_encoders::Response&);
             bool query_linesensor_callback(linesensor::query_linesensor::Request&,
                                            linesensor::query_linesensor::Response&);
+            unsigned int rgb_to_linesensor_val(unsigned char r,
+                                               unsigned char g,
+                                               unsigned char b);
+            void update_linesensor(const wxImage& lines_image, int x, int y, double theta);
 
             ros::NodeHandle node;
 
@@ -145,6 +155,9 @@ namespace scoutsim
             unsigned int bl_ticks;
             unsigned int br_ticks;
 
+            // A vector of the 8 linesensor readings
+            std::vector<unsigned int> linesensor_readings;
+
             // Each scout has a unique id number, which is also displayed on its image.
             int scout_id;
 
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index aa6c66d..4e16d1e 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -92,8 +92,16 @@ namespace scoutsim
         /// @todo This should change.
         meter = scout_images[0].GetHeight();
 
-        base_map_name = map_name;
-        display_map_name = ros::package::getPath("scoutsim") + "/maps/" + map_name + ".bmp";
+        map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
+                           map_name + ".bmp";
+        map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
+                           map_name + "_lines.bmp";
+        display_map_name = map_base_name;
+
+        wxBitmap lines_bitmap;
+        lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str()));
+        lines_image = lines_bitmap.ConvertToImage();
+
         clear();
 
         clear_srv = nh.advertiseService("clear",
@@ -227,15 +235,13 @@ namespace scoutsim
 
     void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
     {
-        display_map_name = ros::package::getPath("scoutsim") + "/maps/" +
-                           base_map_name + ".bmp";
+        display_map_name = map_base_name;
         clear();
     }
 
     void SimFrame::showLines(wxCommandEvent& WXUNUSED(event))
     {
-        display_map_name = ros::package::getPath("scoutsim") + "/maps/" +
-                           base_map_name + "_lines.bmp";
+        display_map_name = map_lines_name;
         clear();
     }
 
@@ -298,7 +304,7 @@ namespace scoutsim
 
         for (; it != end; ++it)
         {
-            it->second->update(0.016, path_dc, path_image,
+            it->second->update(0.016, path_dc, path_image, lines_image,
                                path_dc.GetBackground().GetColour(),
                                state);
         }
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index 98e1966..880acb5 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -103,6 +103,7 @@ namespace scoutsim
             wxTimer* update_timer;
             wxBitmap path_bitmap;
             wxImage path_image;
+            wxImage lines_image;
             wxMemoryDC path_dc;
 
             uint64_t frame_count;
@@ -124,7 +125,8 @@ namespace scoutsim
             float width_in_meters;
             float height_in_meters;
 
-            std::string base_map_name;
+            std::string map_base_name;
+            std::string map_lines_name;
             std::string display_map_name;
     };
 
