commit 43811241b4b22dba6e0abbbbb16af2e6ef041c0d
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Sat Dec 1 19:04:15 2012 -0500

    Added services to set individual control for visualization tools.
    
    However, there is a bug in the sonar viz for multiple scouts, looking into it now.

diff --git a/scout/scoutsim/src/ghost_scout.cpp b/scout/scoutsim/src/ghost_scout.cpp
index bed2c98..f6f4fa4 100644
--- a/scout/scoutsim/src/ghost_scout.cpp
+++ b/scout/scoutsim/src/ghost_scout.cpp
@@ -67,6 +67,7 @@ namespace scoutsim
       , start_y(pos.y)
       , orient(orient)
       , name(scoutname)
+      , is_visible(false)
   {
     scout = wxBitmap(scout_image);
 
@@ -136,8 +137,23 @@ namespace scoutsim
 
   void GhostScout::paint(wxDC& dc)
   {
+      if (!is_visible)
+      {
+          return;
+      }
+
     wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
     dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
         pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
   }
+
+  string GhostScout::get_name()
+  {
+    return name;
+  }
+
+  void GhostScout::set_visible(bool vis)
+  {
+    is_visible = vis;
+  }
 }
diff --git a/scout/scoutsim/src/ghost_scout.h b/scout/scoutsim/src/ghost_scout.h
index 85bd87c..4d81d9a 100644
--- a/scout/scoutsim/src/ghost_scout.h
+++ b/scout/scoutsim/src/ghost_scout.h
@@ -84,6 +84,8 @@ namespace scoutsim
       void paint(wxDC& dc);
 
       void setPosition(const ::messages::ScoutPosition& msg);
+      std::string get_name();
+      void set_visible(bool vis);
 
     private:
       unsigned int rgb_to_grey(unsigned char r,
@@ -104,6 +106,7 @@ namespace scoutsim
 
       ros::Subscriber position;
 
+      bool is_visible;
   };
 }
 
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 08312c0..dc1fd47 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -63,8 +63,8 @@ namespace scoutsim
                  wxBitmap *path_bitmap,
                  float orient)
         : path_bitmap(path_bitmap)
-      , sonar_visual_on(sonar_visual_on)
-      , node (nh)
+          , sonar_visual_on(false)
+          , node (nh)
           , scout_image(scout_image)
           , pos(pos)
           , orient(orient)
@@ -312,8 +312,8 @@ namespace scoutsim
             }
 
             sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
-            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
-            if (isFront) //for some reason isFront = (!isFront) is not working
+            sonar_dc.DrawCircle(wxPoint(d_x,d_y), 2);
+            if (isFront) // @todo for some reason isFront = (!isFront) is not working
             {
                 isFront = FALSE;
             }
@@ -321,7 +321,6 @@ namespace scoutsim
             {
                 isFront = TRUE; 
             }
-                
         }
 
         return d;
@@ -339,9 +338,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);
         unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
-                                          sonar_position + 24,sonar_dc);
+                                          sonar_position + 24, sonar_dc);
 
         // Publish
         sonar::sonar_distance msg;
@@ -393,7 +392,6 @@ namespace scoutsim
     /// TODO remove dt param
     geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
                                         wxMemoryDC& sonar_dc,
-                                        bool sonar_visual, 
                                         const wxImage& path_image,
                                         const wxImage& lines_image,
                                         const wxImage& walls_image,
@@ -401,8 +399,6 @@ 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.
@@ -483,6 +479,7 @@ namespace scoutsim
                          p.theta,sonar_dc);
 
         }
+
         // Figure out (and publish) the color underneath the scout
         {
             //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
@@ -521,4 +518,11 @@ namespace scoutsim
         dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
                       pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
     }
+
+    void Scout::set_sonar_visual(bool on)
+    {
+        /// @todo Remove
+        ROS_INFO("Sonar visual on set.");
+        sonar_visual_on = on;
+    }
 }
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index 3019959..2b5ea49 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -112,15 +112,15 @@ namespace scoutsim
                   const Vector2& pos, wxBitmap *path_bitmap, float orient);
 
             geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
-					 wxMemoryDC& sonar_dc,
-                     bool sonar_visual,
+                                         wxMemoryDC& sonar_dc,
                                          const wxImage& path_image,
                                          const wxImage& lines_image,
                                          const wxImage& walls_image,
                                          wxColour background_color,
-         				 wxColour sonar_color,
+                                         wxColour sonar_color,
                                          world_state state);
             void paint(wxDC& dc);
+            void set_sonar_visual(bool on);
 
         private:
             float absolute_to_mps(int absolute_speed);
diff --git a/scout/scoutsim/src/scoutsim.cpp b/scout/scoutsim/src/scoutsim.cpp
index ed73040..cfc7c2d 100644
--- a/scout/scoutsim/src/scoutsim.cpp
+++ b/scout/scoutsim/src/scoutsim.cpp
@@ -137,8 +137,6 @@ 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)
-    EVT_MENU(ID_TEST, scoutsim::SimFrame::doTest)
 END_EVENT_TABLE()
 
 DECLARE_APP(ScoutApp);
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 41bed1f..2e9b0b4 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -88,7 +88,6 @@ namespace scoutsim
         wxBitmap lines_bitmap;
         wxBitmap walls_bitmap;
         path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
-        sonar_visual_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())))
@@ -113,7 +112,13 @@ namespace scoutsim
         spawn_srv = nh.advertiseService("spawn",
                                         &SimFrame::spawnCallback, this);
         kill_srv = nh.advertiseService("kill",
-                                       &SimFrame::killCallback, this);
+                                        &SimFrame::killCallback, this);
+        set_sonar_viz_srv = nh.advertiseService("set_sonar_viz",
+                                        &SimFrame::setSonarVizCallback, this);
+        set_ghost_srv = nh.advertiseService("set_ghost",
+                                        &SimFrame::setGhostCallback, this);
+        set_teleop_srv = nh.advertiseService("set_teleop",
+                                        &SimFrame::setTeleopCallback, this);
 
         // Subscribe and publisher wirless from robots
         wireless_receive = nh.advertise< ::messages::WirelessPacket>(
@@ -122,7 +127,7 @@ namespace scoutsim
             &SimFrame::wirelessCallback, this);
 
         // Teleop
-        teleop_type = TELEOP_PRECISE;
+        teleop_type = TELEOP_OFF;
         teleop_l_speed = 0;
         teleop_r_speed = 0;
         teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
@@ -136,7 +141,6 @@ 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;
@@ -149,15 +153,11 @@ namespace scoutsim
         menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise"));
         menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid"));
 
-        wxMenu *menuTest = new wxMenu;
-        menuTest->Append(ID_TEST, _("&Scouts go here"));
-
         wxMenuBar *menuBar = new wxMenuBar;
         menuBar->Append(menuFile, _("&File"));
         menuBar->Append(menuSim, _("&Sim"));
         menuBar->Append(menuView, _("&View"));
         menuBar->Append(menuTeleop, _("&Teleop"));
-        menuBar->Append(menuTest, _("T&est"));
 
         SetMenuBar(menuBar);
 
@@ -178,12 +178,11 @@ namespace scoutsim
         std::string name = spawnScout(req.name, req.x, req.y, req.theta);
         if (name.empty())
         {
-            ROS_ERROR("A scout named [%s] already exists", req.name.c_str());
+            ROS_WARN("A scout named [%s] already exists", req.name.c_str());
             return false;
         }
 
         res.name = name;
-
         return true;
     }
 
@@ -193,8 +192,8 @@ namespace scoutsim
         M_Scout::iterator it = scouts.find(req.name);
         if (it == scouts.end())
         {
-            ROS_ERROR("Tried to kill scout [%s], which does not exist",
-                      req.name.c_str());
+            ROS_WARN("Tried to kill scout [%s], which does not exist",
+                     req.name.c_str());
             return false;
         }
 
@@ -203,6 +202,48 @@ namespace scoutsim
         return true;
     }
 
+    bool SimFrame::setSonarVizCallback(SetSonarViz::Request& req,
+                                       SetSonarViz::Response&)
+    {
+        M_Scout::iterator it = scouts.find(req.scout_name);
+        if (it == scouts.end())
+        {
+            ROS_WARN("Tried to set sonar on scout [%s], which does not exist",
+                     req.scout_name.c_str());
+            return false;
+        }
+
+        it->second->set_sonar_visual(req.on);
+        return true;
+    }
+
+    bool SimFrame::setGhostCallback(SetGhost::Request& req,
+                                    SetGhost::Response&)
+    {
+        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
+        {
+            if (ghost_scouts.at(i)->get_name() == req.scout_name)
+            {
+                ghost_scouts.at(i)->set_visible(req.on);
+                return true;
+            }
+        }
+
+        ROS_WARN("Tried to set ghost on scout [%s], which does not exist",
+                 req.scout_name.c_str());
+        return false;
+    }
+
+    bool SimFrame::setTeleopCallback(SetTeleop::Request& req,
+                                     SetTeleop::Response&)
+    {
+        std::stringstream ss;
+        ss << "/" << req.scout_name << "/set_motors";
+        teleop_pub = nh.advertise<motors::set_motors>(ss.str(), 1000);
+
+        return true;
+    }
+
     bool SimFrame::hasScout(const std::string& name)
     {
         return scouts.find(name) != scouts.end();
@@ -214,12 +255,14 @@ namespace scoutsim
         std::string real_name = name;
         if (real_name.empty())
         {
+            // Generate the name scoutX, where X is an increasing number.
             do
             {
                 std::stringstream ss;
                 ss << "scout" << ++id_counter;
                 real_name = ss.str();
-            } while (hasScout(real_name));
+            }
+            while (hasScout(real_name));
         }
         else
         {
@@ -280,17 +323,6 @@ namespace scoutsim
         clear();
     }
 
-    void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event))
-    {
-        sonar_visual_on = not sonar_visual_on;
-        clear();
-    }
-
-    void SimFrame::doTest(wxCommandEvent& WXUNUSED(event))
-    {
-        clear();
-    }
-
     void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
     {
         display_map_name = map_base_name;
@@ -353,7 +385,7 @@ namespace scoutsim
         {
             it->second->paint(dc);
         }
-        for (unsigned int i=0; i<ghost_scouts.size(); ++i)
+        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
         {
             ghost_scouts.at(i)->paint(dc);
         }
@@ -410,13 +442,13 @@ namespace scoutsim
         }
         else if (wxGetKeyState(WXK_LEFT))
         {
-            teleop_l_speed = -TELEOP_PRECISE_SPEED * 4;
-            teleop_r_speed = TELEOP_PRECISE_SPEED * 4;
+            teleop_l_speed = -TELEOP_PRECISE_TURN_SPEED;
+            teleop_r_speed = TELEOP_PRECISE_TURN_SPEED;
         }
         else if (wxGetKeyState(WXK_RIGHT))
         {
-            teleop_l_speed = TELEOP_PRECISE_SPEED * 4;
-            teleop_r_speed = -TELEOP_PRECISE_SPEED * 4;
+            teleop_l_speed = TELEOP_PRECISE_TURN_SPEED;
+            teleop_r_speed = -TELEOP_PRECISE_TURN_SPEED;
         }
     }
 
@@ -531,14 +563,14 @@ namespace scoutsim
         for (; it != end; ++it)
         {
             it->second->update(SIM_TIME_REFRESH_RATE,
-                               path_dc,sonar_dc,sonar_visual_on,
+                               path_dc, sonar_dc,
                                path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
                                sonar_dc.GetBackground().GetColour(),
                                state);
         }
 
-        for (unsigned int i=0; i<ghost_scouts.size(); ++i)
+        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
         {
             ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
                 path_dc.GetBackground().GetColour(), state);
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index 8af1be4..7163a0d 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -58,6 +58,9 @@
 #include <std_srvs/Empty.h>
 #include <scoutsim/Spawn.h>
 #include <scoutsim/Kill.h>
+#include <scoutsim/SetSonarViz.h>
+#include <scoutsim/SetGhost.h>
+#include <scoutsim/SetTeleop.h>
 #include <motors/set_motors.h>
 #include <map>
 
@@ -76,11 +79,11 @@
 #define ID_TELEOP_NONE 7
 #define ID_TELEOP_PRECISE 8
 #define ID_TELEOP_FLUID 9
-#define ID_SONAR 10
-#define ID_TEST 11
 
 // Absolute speeds (-128 - 127)
-#define TELEOP_PRECISE_SPEED 50
+/// @todo: Clean this up a little; we should be risking overflowing shorts.
+#define TELEOP_PRECISE_SPEED 60
+#define TELEOP_PRECISE_TURN_SPEED 124
 #define TELEOP_FLUID_MAX_SPEED 100
 #define TELEOP_FLUID_INC 8
 
@@ -109,8 +112,6 @@ namespace scoutsim
             void stopTeleop(wxCommandEvent& event);
             void startTeleopPrecise(wxCommandEvent& event);
             void startTeleopFluid(wxCommandEvent& event);
-            void showSonar(wxCommandEvent& event);
-            void doTest(wxCommandEvent& event);
 
             DECLARE_EVENT_TABLE()
 
@@ -134,6 +135,10 @@ namespace scoutsim
             bool spawnCallback(Spawn::Request&, Spawn::Response&);
             bool killCallback(Kill::Request&, Kill::Response&);
 
+            bool setSonarVizCallback(SetSonarViz::Request&, SetSonarViz::Response&);
+            bool setGhostCallback(SetGhost::Request&, SetGhost::Response&);
+            bool setTeleopCallback(SetTeleop::Request&, SetTeleop::Response&);
+
             void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg);
 
             ros::Subscriber wireless_send;
@@ -165,6 +170,9 @@ namespace scoutsim
             ros::ServiceServer reset_srv;
             ros::ServiceServer spawn_srv;
             ros::ServiceServer kill_srv;
+            ros::ServiceServer set_sonar_viz_srv;
+            ros::ServiceServer set_ghost_srv;
+            ros::ServiceServer set_teleop_srv;
 
             typedef std::map<std::string, ScoutPtr> M_Scout;
             M_Scout scouts;
@@ -176,8 +184,6 @@ namespace scoutsim
             float width_in_meters;
             float height_in_meters;
 
-            bool sonar_visual_on;
-
             std::string map_base_name;
             std::string map_lines_name;
             std::string map_walls_name;
diff --git a/scout/scoutsim/srv/Spawn.srv b/scout/scoutsim/srv/Spawn.srv
index b8eeaee..7cce73b 100644
--- a/scout/scoutsim/srv/Spawn.srv
+++ b/scout/scoutsim/srv/Spawn.srv
@@ -3,4 +3,4 @@ float32 y
 float32 theta
 string name # Optional.  A unique name will be created and returned if this is empty
 ---
-string name
\ No newline at end of file
+string name
