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/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);
