commit 0e77683c65f392af332f84aa9b31eb8614290a55
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Sun Oct 28 21:24:47 2012 -0400

    Added built-in scoutsim teleop!

diff --git a/scout/scoutsim/src/scoutsim.cpp b/scout/scoutsim/src/scoutsim.cpp
index 967bb68..cfc7c2d 100644
--- a/scout/scoutsim/src/scoutsim.cpp
+++ b/scout/scoutsim/src/scoutsim.cpp
@@ -134,6 +134,9 @@ BEGIN_EVENT_TABLE(scoutsim::SimFrame, wxFrame)
     EVT_MENU(ID_MAP, scoutsim::SimFrame::showMap)
     EVT_MENU(ID_LINES, scoutsim::SimFrame::showLines)
     EVT_MENU(ID_WALLS, scoutsim::SimFrame::showWalls)
+    EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop)
+    EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise)
+    EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid)
 END_EVENT_TABLE()
 
 DECLARE_APP(ScoutApp);
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 325f558..16f9f58 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -135,6 +135,12 @@ namespace scoutsim
         wireless_send = nh.subscribe("/wireless/send", 1000,
             &SimFrame::wirelessCallback, this);
 
+        // Teleop
+        teleop_type = TELEOP_OFF;
+        teleop_l_speed = 0;
+        teleop_r_speed = 0;
+        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
+
         ROS_INFO("Starting scoutsim with node name %s",
                  ros::this_node::getName().c_str()) ;
 
@@ -151,10 +157,16 @@ namespace scoutsim
         menuView->Append(ID_LINES, _("&Lines"));
         menuView->Append(ID_WALLS, _("&Walls"));
 
+        wxMenu *menuTeleop = new wxMenu;
+        menuTeleop->Append(ID_TELEOP_NONE, _("&None"));
+        menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise"));
+        menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid"));
+
         wxMenuBar *menuBar = new wxMenuBar;
         menuBar->Append(menuFile, _("&File"));
         menuBar->Append(menuSim, _("&Sim"));
         menuBar->Append(menuView, _("&View"));
+        menuBar->Append(menuTeleop, _("&Teleop"));
 
         SetMenuBar(menuBar);
 
@@ -293,6 +305,8 @@ namespace scoutsim
     {
         ros::spinOnce();
 
+        teleop();
+
         updateScouts();
 
         if (!ros::ok())
@@ -315,6 +329,134 @@ namespace scoutsim
         }
     }
 
+    void SimFrame::stopTeleop(wxCommandEvent& event)
+    {
+        teleop_type = TELEOP_OFF;
+        teleop_l_speed = 0;
+        teleop_r_speed = 0;
+    }
+
+    void SimFrame::startTeleopPrecise(wxCommandEvent& event)
+    {
+        teleop_type = TELEOP_PRECISE;
+        teleop_l_speed = 0;
+        teleop_r_speed = 0;
+    }
+
+    void SimFrame::startTeleopFluid(wxCommandEvent& event)
+    {
+        teleop_type = TELEOP_FLUID;
+        teleop_l_speed = 0;
+        teleop_r_speed = 0;
+        teleop_fluid_speed = 0;
+        teleop_fluid_omega = 0;
+    }
+
+    void SimFrame::teleop_move_precise()
+    {
+        // Default to stop
+        teleop_l_speed = 0;
+        teleop_r_speed = 0;
+
+        if (wxGetKeyState(WXK_UP))
+        {
+            teleop_l_speed = TELEOP_PRECISE_SPEED;
+            teleop_r_speed = TELEOP_PRECISE_SPEED;
+        }
+        else if (wxGetKeyState(WXK_DOWN))
+        {
+            teleop_l_speed = -TELEOP_PRECISE_SPEED;
+            teleop_r_speed = -TELEOP_PRECISE_SPEED;
+        }
+        else if (wxGetKeyState(WXK_LEFT))
+        {
+            teleop_l_speed = -TELEOP_PRECISE_SPEED;
+            teleop_r_speed = TELEOP_PRECISE_SPEED;
+        }
+        else if (wxGetKeyState(WXK_RIGHT))
+        {
+            teleop_l_speed = TELEOP_PRECISE_SPEED;
+            teleop_r_speed = -TELEOP_PRECISE_SPEED;
+        }
+    }
+
+    void SimFrame::teleop_move_fluid()
+    {
+        if (wxGetKeyState(WXK_UP))
+        {
+            teleop_fluid_speed += 2;
+        }
+        else if (wxGetKeyState(WXK_DOWN))
+        {
+            teleop_fluid_speed -= 2;
+        }
+        else if (teleop_fluid_speed > 0)
+        {
+            teleop_fluid_speed -= 1;
+        }
+        else if (teleop_fluid_speed < 0)
+        {
+            teleop_fluid_speed += 1;
+        }
+
+        if (wxGetKeyState(WXK_LEFT))
+        {
+            teleop_fluid_omega -= 1;
+        }
+        else if (wxGetKeyState(WXK_RIGHT))
+        {
+            teleop_fluid_omega += 1;
+        }
+        else if (teleop_fluid_omega > 0)
+        {
+            teleop_fluid_omega -= 1;
+        }
+        else if (teleop_fluid_omega < 0)
+        {
+            teleop_fluid_omega += 1;
+        }
+
+        if (teleop_fluid_speed > TELEOP_FLUID_MAX_SPEED)
+        {
+            teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED;
+        }
+        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
+        {
+            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
+        }
+
+        teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega;
+        teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega;
+    }
+
+    void SimFrame::teleop()
+    {
+        switch (teleop_type)
+        {
+            case TELEOP_OFF:
+                return;
+            case TELEOP_PRECISE:
+                teleop_move_precise();
+                break;
+            case TELEOP_FLUID:
+                teleop_move_fluid();
+                break;
+        }
+
+        motors::set_motors msg;
+        msg.fl_set = true;
+        msg.fr_set = true;
+        msg.bl_set = true;
+        msg.br_set = true;
+
+        msg.fl_speed = teleop_l_speed;
+        msg.fr_speed = teleop_r_speed;
+        msg.bl_speed = teleop_l_speed;
+        msg.br_speed = teleop_r_speed;
+
+        teleop_pub.publish(msg);
+    }
+
     void SimFrame::updateScouts()
     {
         if (last_scout_update.isZero())
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index a454225..0104874 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -49,12 +49,14 @@
 #include <wx/event.h>
 #include <wx/timer.h>
 #include <wx/string.h>
+#include <wx/utils.h>
 
 #include <ros/ros.h>
 
 #include <std_srvs/Empty.h>
 #include <scoutsim/Spawn.h>
 #include <scoutsim/Kill.h>
+#include <motors/set_motors.h>
 #include <map>
 
 #include "scout.h"
@@ -68,6 +70,17 @@
 #define ID_MAP 4
 #define ID_LINES 5
 #define ID_WALLS 6
+#define ID_TELEOP_NONE 7
+#define ID_TELEOP_PRECISE 8
+#define ID_TELEOP_FLUID 9
+
+#define TELEOP_PRECISE_SPEED 30
+#define TELEOP_FLUID_MAX_SPEED 80
+
+// Teleop types
+#define TELEOP_OFF 0
+#define TELEOP_PRECISE 1
+#define TELEOP_FLUID 2
 
 namespace scoutsim
 {
@@ -86,6 +99,9 @@ namespace scoutsim
             void showMap(wxCommandEvent& event);
             void showLines(wxCommandEvent& event);
             void showWalls(wxCommandEvent& event);
+            void stopTeleop(wxCommandEvent& event);
+            void startTeleopPrecise(wxCommandEvent& event);
+            void startTeleopFluid(wxCommandEvent& event);
 
             DECLARE_EVENT_TABLE()
 
@@ -93,6 +109,10 @@ namespace scoutsim
             void onUpdate(wxTimerEvent& evt);
             void onPaint(wxPaintEvent& evt);
 
+            void teleop_move_precise();
+            void teleop_move_fluid();
+            void teleop();
+
             void updateScouts();
             void clear();
             bool hasScout(const std::string& name);
@@ -107,6 +127,15 @@ namespace scoutsim
             ros::Subscriber wireless_send;
             ros::Publisher wireless_receive;
 
+            // Teleop
+            short teleop_l_speed;
+            short teleop_r_speed;
+            ros::Publisher teleop_pub;
+            int teleop_type;
+
+            short teleop_fluid_speed;
+            short teleop_fluid_omega;
+
             ros::NodeHandle nh;
             wxTimer* update_timer;
             wxBitmap path_bitmap;
@@ -114,7 +143,7 @@ namespace scoutsim
             wxImage lines_image;
             wxImage walls_image;
             wxMemoryDC path_dc;
-	    wxMemoryDC sonar_dc;
+            wxMemoryDC sonar_dc;
 
             uint64_t frame_count;
 
