commit 6639ce9c5b5ca4644e9f66e71b7d79bc24cf4fb0
Author: viki <viki@c3po.(none)>
Date:   Sun Oct 28 16:58:49 2012 -0500

    Edited Sonar to display points on simulator.
    
    Changed rate of sonar scan to 0.5s
    Made a new wxDC graphics object for sonar

diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 4664d76..f7c1f3f 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -60,8 +60,10 @@ namespace scoutsim
     Scout::Scout(const ros::NodeHandle& nh,
                  const wxImage& scout_image,
                  const Vector2& pos,
+                 wxBitmap *path_bitmap,
                  float orient)
-        : node (nh)
+        : path_bitmap(path_bitmap)
+	  , node (nh)
           , scout_image(scout_image)
           , pos(pos)
           , orient(orient)
@@ -191,16 +193,18 @@ namespace scoutsim
     }
 
     unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
-                                    double robot_theta, int sonar_pos)
+                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
     {
         double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
         unsigned int d = 0;
 
         unsigned int reading = 0;
+	int d_x = 0;
+	int d_y = 0;
         do
         {
-            int d_x = x + (int) floor(d * cos(angle));
-            int d_y = y + (int) floor(d * sin(angle));
+            d_x = x + (int) floor(d * cos(angle));
+            d_y = y + (int) floor(d * sin(angle));
 
             // Out of image boundary
             if (d_x < 0 || d_x >= walls_image.GetWidth() ||
@@ -219,20 +223,31 @@ namespace scoutsim
             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++;
         }
         /// @todo Consider using different cutoffs for different features
         while (reading < 128); /// @todo Get rid of hardcoded stuff like this
+        
+        // draw a circle at the wall_x, wall_y where reading > 128
+        sonar_dc.SelectObject(*path_bitmap);
+        sonar_dc.SetBrush(*wxRED_BRUSH); //old value
+        sonar_dc.DrawCircle(wxPoint(old_dx,old_dy), 2);
+
+        sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
+        sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
+        old_dx = d_x;
+	old_dy = d_y;
 
         return d;
     }
 
     // x and y is current position of the sonar
     void Scout::update_sonar(const wxImage& walls_image, int x, int y,
-                             double robot_theta)
+                             double robot_theta,wxMemoryDC& sonar_dc)
     {
         // Only rotate the sonar at the correct rate.
         if (ros::Time::now() - last_sonar_time < sonar_tick_time)
@@ -242,9 +257,9 @@ namespace scoutsim
         last_sonar_time = ros::Time::now();
 
         unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
-                                           sonar_position);
+                                           sonar_position,sonar_dc);
         unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
-                                          sonar_position + 24);
+                                          sonar_position + 24,sonar_dc);
 
         // Publish
         sonar::sonar_distance msg;
@@ -294,10 +309,12 @@ namespace scoutsim
     /// Sends back the position of this scout so scoutsim can save
     /// the world state
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
+					wxMemoryDC& sonar_dc,
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
                                         const wxImage& walls_image,
                                         wxColour background_color,
+					wxColour sonar_color,
                                         world_state state)
     {
         // Assume that the two motors on the same side will be set to
@@ -371,7 +388,7 @@ namespace scoutsim
         update_sonar(walls_image,
                      canvas_x + scoutsim::SCOUT_SONAR_X,
                      canvas_y + scoutsim::SCOUT_SONAR_Y,
-                     p.theta);
+                     p.theta,sonar_dc);
 
         // Figure out (and publish) the color underneath the scout
         {
@@ -390,6 +407,7 @@ namespace scoutsim
         {
             if (pos != old_pos)
             {
+                path_dc.SelectObject(*path_bitmap);
                 path_dc.SetPen(pen);
                 path_dc.DrawLine(pos.x * meter, pos.y * meter,
                                  old_pos.x * meter, old_pos.y * meter);
