commit 68b23184aa0f17dabaef999c99dd25e009fb3af5
Author: viki <viki@c3po.(none)>
Date:   Fri Nov 2 12:03:46 2012 -0500

    Scoutsim: Added Sonar Toggle
    
    Sonar display can now be toggled via Menu->Sim->Sonar

diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index f7c1f3f..9032fce 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -63,6 +63,7 @@ namespace scoutsim
                  wxBitmap *path_bitmap,
                  float orient)
         : path_bitmap(path_bitmap)
+	  , sonar_on(sonar_on)
 	  , node (nh)
           , scout_image(scout_image)
           , pos(pos)
@@ -193,7 +194,7 @@ namespace scoutsim
     }
 
     unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
-                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
+                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
     {
         double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
         unsigned int d = 0;
@@ -232,22 +233,24 @@ namespace scoutsim
         /// @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;
-
+	if(sonar_on == TRUE)
+	{        
+		// 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,wxMemoryDC& sonar_dc)
+                             double robot_theta,wxMemoryDC& sonar_dc, bool sonar_on)
     {
         // Only rotate the sonar at the correct rate.
         if (ros::Time::now() - last_sonar_time < sonar_tick_time)
@@ -257,9 +260,9 @@ namespace scoutsim
         last_sonar_time = ros::Time::now();
 
         unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
-                                           sonar_position,sonar_dc);
+                                           sonar_position,sonar_dc, sonar_on);
         unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
-                                          sonar_position + 24,sonar_dc);
+                                          sonar_position + 24,sonar_dc, sonar_on);
 
         // Publish
         sonar::sonar_distance msg;
@@ -310,6 +313,7 @@ namespace scoutsim
     /// the world state
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
 					wxMemoryDC& sonar_dc,
+					bool sonar_on,
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
                                         const wxImage& walls_image,
@@ -388,7 +392,7 @@ namespace scoutsim
         update_sonar(walls_image,
                      canvas_x + scoutsim::SCOUT_SONAR_X,
                      canvas_y + scoutsim::SCOUT_SONAR_Y,
-                     p.theta,sonar_dc);
+                     p.theta,sonar_dc,sonar_on);
 
         // Figure out (and publish) the color underneath the scout
         {
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index c56ba23..4d26fe4 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -113,7 +113,8 @@ namespace scoutsim
                   const Vector2& pos, wxBitmap *path_bitmap, float orient);
 
             geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
-					wxMemoryDC& sonar_dc,
+					 wxMemoryDC& sonar_dc,
+					 bool sonar_on,
                                          const wxImage& path_image,
                                          const wxImage& lines_image,
                                          const wxImage& walls_image,
@@ -134,14 +135,15 @@ namespace scoutsim
                                      unsigned char g,
                                      unsigned char b);
             unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
-                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc);
+                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
             void update_sonar(const wxImage& walls_image, int x, int y,
-                              double robot_theta, wxMemoryDC& sonar_dc);
+                              double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
             void update_linesensor(const wxImage& lines_image, int x, int y,
                                    double theta);
 	    int old_dx;
 	    int old_dy;
 	    wxBitmap *path_bitmap;
+	    bool sonar_on;
 
             ros::NodeHandle node;
 
diff --git a/scout/scoutsim/src/scoutsim.cpp b/scout/scoutsim/src/scoutsim.cpp
index cfc7c2d..821558d 100644
--- a/scout/scoutsim/src/scoutsim.cpp
+++ b/scout/scoutsim/src/scoutsim.cpp
@@ -137,6 +137,7 @@ BEGIN_EVENT_TABLE(scoutsim::SimFrame, wxFrame)
     EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop)
     EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise)
     EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid)
+    EVT_MENU(ID_SONAR, scoutsim::SimFrame::showSonar)
 END_EVENT_TABLE()
 
 DECLARE_APP(ScoutApp);
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 16f9f58..113ac69 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -103,6 +103,7 @@ namespace scoutsim
         wxBitmap lines_bitmap;
         wxBitmap walls_bitmap;
         path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
+        sonar_on = TRUE;
 
         // Try to load the file; if it fails, make a new blank file
         if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
@@ -150,6 +151,7 @@ namespace scoutsim
         menuFile->Append(ID_QUIT, _("E&xit"));
 
         wxMenu *menuSim = new wxMenu;
+        menuSim->Append(ID_SONAR, _("S&onar"));
         menuSim->Append(ID_CLEAR, _("&Clear"));
 
         wxMenu *menuView = new wxMenu;
@@ -268,6 +270,13 @@ namespace scoutsim
         clear();
     }
 
+    void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event))
+    {
+        sonar_on = not sonar_on;
+	clear();
+    }
+
+
     void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
     {
         display_map_name = map_base_name;
@@ -480,7 +489,7 @@ namespace scoutsim
 
         for (; it != end; ++it)
         {
-            it->second->update(0.016, path_dc,sonar_dc,
+            it->second->update(0.016, path_dc,sonar_dc,sonar_on,
                                path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
 			       sonar_dc.GetBackground().GetColour(),
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index 0104874..742953c 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -73,6 +73,7 @@
 #define ID_TELEOP_NONE 7
 #define ID_TELEOP_PRECISE 8
 #define ID_TELEOP_FLUID 9
+#define ID_SONAR 10
 
 #define TELEOP_PRECISE_SPEED 30
 #define TELEOP_FLUID_MAX_SPEED 80
@@ -102,6 +103,7 @@ namespace scoutsim
             void stopTeleop(wxCommandEvent& event);
             void startTeleopPrecise(wxCommandEvent& event);
             void startTeleopFluid(wxCommandEvent& event);
+	    void showSonar(wxCommandEvent& event);
 
             DECLARE_EVENT_TABLE()
 
@@ -164,6 +166,8 @@ namespace scoutsim
             float width_in_meters;
             float height_in_meters;
 
+	    bool sonar_on;
+
             std::string map_base_name;
             std::string map_lines_name;
             std::string map_walls_name;
