commit a2e6bd4c826a26463a1cde2b2ed7e2c16ff43bdd
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Thu Oct 25 22:02:43 2012 -0400

    Added sonar, though it looks buggy.
    
    Use sonar_viz to continue debugging and make sonar work!

diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 8d0a380..e7a0b0a 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -83,6 +83,7 @@ namespace scoutsim
 
         pose_pub = node.advertise<Pose>("pose", 1);
         color_pub = node.advertise<Color>("color_sensor", 1);
+        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
         set_pen_srv = node.advertiseService("set_pen",
                                             &Scout::setPenCallback,
                                             this);
@@ -102,6 +103,13 @@ namespace scoutsim
         }
 
         meter = scout.GetHeight();
+
+        // Initialize sonar
+        sonar_position = 0;
+        sonar_stop_l = 0;
+        sonar_stop_r = 23;
+        sonar_direction = 1;
+        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
     }
 
     /**
@@ -177,9 +185,9 @@ namespace scoutsim
     }
 
     // Scale to linesensor value
-    unsigned int Scout::rgb_to_linesensor_val(unsigned char r,
-                                              unsigned char g,
-                                              unsigned char b)
+    unsigned int Scout::rgb_to_grey(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;
@@ -188,7 +196,82 @@ namespace scoutsim
         return 255 - grey;
     }
 
-    void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta)
+    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
+                                    double robot_theta, int sonar_pos)
+    {
+        double angle = robot_theta + (180.0 * ((float) sonar_pos) / 24.0);
+        unsigned int d = 0;
+
+        unsigned int reading = 0;
+        do
+        {
+            int d_x = x + (int) floor(d * cos(angle));
+            int d_y = y + (int) floor(d * sin(angle));
+
+            // Out of image boundary
+            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
+                d_y < 0 || d_y >= walls_image.GetHeight())
+            {
+                return d;
+            }
+
+            // Max range
+            if (d > scoutsim::SONAR_MAX_RANGE)
+            {
+                return d;
+            }
+
+            // Get the sonar reading at the current position of the sonar
+            unsigned char r = walls_image.GetRed(d_x, d_y);
+            unsigned char g = walls_image.GetGreen(d_x, d_y);
+            unsigned char b = walls_image.GetBlue(d_x, d_y);
+
+            reading = rgb_to_grey(r, g, b);
+
+            d++;
+        }
+        while (reading < 128);
+
+        return d;
+    }
+
+    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
+                             double robot_theta)
+    {
+        // Only rotate the sonar at the correct rate.
+        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
+        {
+            return;
+        }
+        last_sonar_time = ros::Time::now();
+
+        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
+                                           sonar_position);
+        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
+                                          sonar_position + 24);
+
+        // Publish
+        sonar::sonar_distance msg;
+        msg.pos = sonar_position;
+        msg.distance0 = d_front;
+        msg.distance1 = d_back;
+
+        sonar_pub.publish(msg);
+
+        // Update the sonar rotation
+        if (sonar_position + sonar_direction <= sonar_stop_r &&
+            sonar_position + sonar_direction >= sonar_stop_l)
+        {
+            sonar_position = sonar_position + sonar_direction;
+        }
+        else
+        {
+            sonar_direction = -sonar_direction;
+        }
+    }
+
+    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
+                                  double robot_theta)
     {
         linesensor_readings.clear();
 
@@ -197,14 +280,16 @@ namespace scoutsim
         {
             double offset = -(scout_image.GetWidth() / 2) + s * spacing;
 
-            int sensor_x = (int) (x - LNSNSR_D * cos(theta) - offset * sin(theta));
-            int sensor_y = (int) (y + LNSNSR_D * sin(theta) - offset * cos(theta));
+            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
+                                  offset * sin(robot_theta));
+            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
+                                  offset * cos(robot_theta));
 
             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);
+            unsigned int reading = rgb_to_grey(r, g, b);
 
             linesensor_readings.push_back(reading);
         }
@@ -215,6 +300,7 @@ namespace scoutsim
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
+                                        const wxImage& walls_image,
                                         wxColour background_color,
                                         world_state state)
     {
@@ -286,6 +372,10 @@ namespace scoutsim
         pose_pub.publish(p);
 
         update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
+        update_sonar(walls_image,
+                     canvas_x + scoutsim::SCOUT_SONAR_X,
+                     canvas_y + scoutsim::SCOUT_SONAR_Y,
+                     p.theta);
 
         // Figure out (and publish) the color underneath the scout
         {
