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/CMakeLists.txt b/scout/scoutsim/CMakeLists.txt
index b208ab4..9eff09b 100644
--- a/scout/scoutsim/CMakeLists.txt
+++ b/scout/scoutsim/CMakeLists.txt
@@ -36,5 +36,6 @@ target_link_libraries(scoutsim_node ${wxWidgets_LIBRARIES})
 #rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
 #rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
 #rosbuild_add_executable(mimic tutorials/mimic.cpp)
+rosbuild_add_executable(sonar_viz src/sonar_viz.cpp)
 rosbuild_add_executable(scout_teleop src/scout_teleop.cpp)
 rosbuild_add_executable(scout_teleop_userinput src/scout_teleop_userinput.cpp)
diff --git a/scout/scoutsim/manifest.xml b/scout/scoutsim/manifest.xml
index 7ddc42a..91d2ec6 100644
--- a/scout/scoutsim/manifest.xml
+++ b/scout/scoutsim/manifest.xml
@@ -16,6 +16,7 @@
   <depend package="motors" />
   <depend package="encoders" />
   <depend package="linesensor" />
+  <depend package="sonar" />
   <depend package="geometry_msgs" />
   <depend package="messages"/>
   
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
         {
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index 200f2be..e6bea76 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -55,6 +55,7 @@
 #include <motors/set_motors.h>
 #include <encoders/query_encoders.h>
 #include <linesensor/query_linesensor.h>
+#include <sonar/sonar_distance.h>
 
 #include <scoutsim/Pose.h>
 #include <scoutsim/SetPen.h>
@@ -65,8 +66,7 @@
 #include <wx/wx.h>
 
 #include "scoutsim_internal.h"
-
-//#include "scout_constants.h"
+#include "scout_constants.h"
 
 #define PI 3.14159265
 
@@ -115,6 +115,7 @@ namespace scoutsim
             geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
                                          const wxImage& path_image,
                                          const wxImage& lines_image,
+                                         const wxImage& walls_image,
                                          wxColour background_color,
                                          world_state state);
             void paint(wxDC& dc);
@@ -128,10 +129,15 @@ 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);
+            unsigned int rgb_to_grey(unsigned char r,
+                                     unsigned char g,
+                                     unsigned char b);
+            unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
+                                     double robot_theta, int sonar_pos);
+            void update_sonar(const wxImage& walls_image, int x, int y,
+                              double robot_theta);
+            void update_linesensor(const wxImage& lines_image, int x, int y,
+                                   double theta);
 
             ros::NodeHandle node;
 
@@ -155,6 +161,14 @@ namespace scoutsim
             unsigned int bl_ticks;
             unsigned int br_ticks;
 
+            int sonar_position;
+            int sonar_stop_l;
+            int sonar_stop_r;
+            int sonar_direction;
+            // The last time the sonar changed its position.
+            ros::Time last_sonar_time;
+            ros::Duration sonar_tick_time;
+
             // A vector of the 8 linesensor readings
             std::vector<unsigned int> linesensor_readings;
 
@@ -167,6 +181,7 @@ namespace scoutsim
             ros::Subscriber motors_sub;
             ros::Publisher pose_pub;
             ros::Publisher color_pub;
+            ros::Publisher sonar_pub;
             ros::ServiceServer set_pen_srv;
             ros::ServiceServer query_encoders_srv;
             ros::ServiceServer query_linesensor_srv;
diff --git a/scout/scoutsim/src/scout_constants.h b/scout/scoutsim/src/scout_constants.h
index ea8d417..7287bcc 100644
--- a/scout/scoutsim/src/scout_constants.h
+++ b/scout/scoutsim/src/scout_constants.h
@@ -56,4 +56,8 @@ namespace scoutsim
     // Position of sonar relative to robot center
     const float SCOUT_SONAR_X = -0.01;
     const float SCOUT_SONAR_Y = 0.0;
+
+    const unsigned int SONAR_MAX_RANGE = 255;
+    // Time it takes for the sonar to spin from position 0 to position 23
+    const float SONAR_HALF_SPIN_TIME = 24.0;
 }
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 3ab0d77..8d09d6b 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -323,7 +323,8 @@ namespace scoutsim
 
         for (; it != end; ++it)
         {
-            it->second->update(0.016, path_dc, path_image, lines_image,
+            it->second->update(0.016, path_dc,
+                               path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
                                state);
         }
diff --git a/scout/scoutsim/src/sonar_viz.cpp b/scout/scoutsim/src/sonar_viz.cpp
new file mode 100644
index 0000000..9af1a4f
--- /dev/null
+++ b/scout/scoutsim/src/sonar_viz.cpp
@@ -0,0 +1,47 @@
+#include <iostream>
+#include <iomanip>
+
+#include <ros/ros.h>
+#include <sonar/sonar_distance.h>
+
+#include "scout_teleop.h"
+
+#define QUEUE_SIZE 10
+
+using namespace std;
+
+vector<unsigned int> distances;
+
+void sonar_distance_callback(const sonar::sonar_distance::ConstPtr& msg)
+{
+    distances[msg->pos] = msg->distance0;
+    distances[msg->pos + 24] = msg->distance1;
+
+    cout << endl << endl << endl << "[ ";
+    for (int i = 0; i < 48; i++)
+    {
+        cout << setw(3) << distances[i] << " ";
+    }
+    cout << "]" << endl;
+}
+
+int main(int argc, char **argv)
+{
+    if (argc < 2)
+    {
+        cout << "Usage: " << argv[0] << " <scout_name>" << endl;
+        exit(1);
+    }
+
+    ros::init(argc, argv, "sonar_viz");
+
+    distances = vector<unsigned int>(48, 0);
+    ros::NodeHandle n;
+
+    string scoutname = argv[1];
+    ros::Subscriber sonar_distance_sub = n.subscribe(scoutname + "/sonar_distance",
+                                                     QUEUE_SIZE,
+                                                     sonar_distance_callback);
+
+    ros::spin();
+}
diff --git a/scout/sonar/msg/sonar_distance.msg b/scout/sonar/msg/sonar_distance.msg
index 574983b..3d2c9da 100644
--- a/scout/sonar/msg/sonar_distance.msg
+++ b/scout/sonar/msg/sonar_distance.msg
@@ -5,6 +5,6 @@ Header header
 int8 pos
 
 # Distance reading from the front sonar in mm
-int8 distance0
+uint8 distance0
 # Distance reading from the back sonar in mm
-int8 distance1
+uint8 distance1
