commit eb9cff771c06a2ed3f37c591ed4c1ee0a20df76d
Author: Hui Jun Tay <viki@c3po.wv.cc.cmu.edu>
Date:   Fri Nov 16 17:44:24 2012 -0600

    Fixed sonar bug (corrected y direction)
    Added sonar_toggle and set_scan command and callbacks
    Cleaned up sonar_visual toggle code

diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 018ecec..b0b8601 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -63,7 +63,7 @@ namespace scoutsim
                  wxBitmap *path_bitmap,
                  float orient)
         : path_bitmap(path_bitmap)
-	  , sonar_on(sonar_on)
+	  , sonar_visual_on(sonar_visual_on)
 	  , node (nh)
           , scout_image(scout_image)
           , pos(pos)
@@ -90,6 +90,12 @@ namespace scoutsim
         set_pen_srv = node.advertiseService("set_pen",
                                             &Scout::setPenCallback,
                                             this);
+        toggle_sonar_srv = node.advertiseService("sonar_toggle",
+                                            &Scout::handle_sonar_toggle,
+                                            this);
+        set_sonar_srv = node.advertiseService("sonar_set_scan",
+                                            &Scout::handle_sonar_set_scan,
+                                            this);
         query_encoders_srv =
             node.advertiseService("query_encoders",
                                   &Scout::query_encoders_callback,
@@ -142,6 +148,54 @@ namespace scoutsim
         }
 
     }
+	//scout sonar callback
+
+    bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
+	                     sonar::sonar_toggle::Response &res)
+    {
+        if (req.set_on && !sonar_on)
+        {
+	        ROS_INFO("Turning on the sonar");
+            sonar_on = true;
+
+        }
+        else if (!req.set_on && sonar_on)
+        {
+	        ROS_INFO("Turning off the sonar");
+            sonar_on = false;
+        }
+        else
+        {
+        ROS_INFO("Sonar state remains unchanged");
+        }
+
+    res.ack = true;
+    return true;
+    }
+
+
+bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
+                           sonar::sonar_set_scan::Response &res)
+{
+    // Code to set the sonar to scan from
+    // req.stop_l to req.stop_r
+    if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
+    {
+    ROS_INFO("Setting sonar scan range to [%i, %i]",
+             req.stop_l,
+             req.stop_r);
+    sonar_stop_l = req.stop_l;
+    sonar_stop_r = req.stop_r;
+    sonar_position = req.stop_l;
+    sonar_direction = 1;
+    res.ack = true;
+    }
+    else
+    {
+    ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
+    }
+    return true;
+}
 
     bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
                                scoutsim::SetPen::Response&)
@@ -194,18 +248,18 @@ namespace scoutsim
     }
 
     unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
-                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
+                                    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;
+	    int d_x = 0;
+	    int d_y = 0;
         do
         {
             d_x = x + (int) floor(d * cos(angle));
-            d_y = y + (int) floor(d * sin(angle));
+            d_y = y - (int) floor(d * sin(angle));
 
             // Out of image boundary
             if (d_x < 0 || d_x >= walls_image.GetWidth() ||
@@ -233,8 +287,8 @@ namespace scoutsim
         /// @todo Consider using different cutoffs for different features
         while (reading < 128); /// @todo Get rid of hardcoded stuff like this
         
-	if(sonar_on)
-	{       
+	if(sonar_visual_on)
+	{   
 		if(isFront)
 		{
 			// draw a circle at the wall_x, wall_y where reading > 128
@@ -271,7 +325,7 @@ namespace scoutsim
 
     // 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, bool sonar_on)
+                             double robot_theta,wxMemoryDC& sonar_dc)
     {
         // Only rotate the sonar at the correct rate.
         if (ros::Time::now() - last_sonar_time < sonar_tick_time)
@@ -281,9 +335,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_on);
+                                           sonar_position,sonar_dc);
         unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
-                                          sonar_position + 24,sonar_dc, sonar_on);
+                                          sonar_position + 24,sonar_dc);
 
         // Publish
         sonar::sonar_distance msg;
@@ -334,7 +388,7 @@ namespace scoutsim
     /// the world state
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
 					wxMemoryDC& sonar_dc,
-					bool sonar_on,
+                    bool sonar_visual,
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
                                         const wxImage& walls_image,
@@ -342,6 +396,8 @@ namespace scoutsim
 					wxColour sonar_color,
                                         world_state state)
     {
+
+        sonar_visual_on = sonar_visual;
         // Assume that the two motors on the same side will be set to
         // roughly the same speed. Does not account for slip conditions
         // when they are set to different speeds.
@@ -410,11 +466,14 @@ 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,sonar_dc,sonar_on);
+        if (sonar_on)
+        {
+            update_sonar(walls_image,
+                         canvas_x + scoutsim::SCOUT_SONAR_X,
+                         canvas_y + scoutsim::SCOUT_SONAR_Y,
+                         p.theta,sonar_dc);
 
+        }
         // Figure out (and publish) the color underneath the scout
         {
             //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
