commit 60a90290ba7db8bc65d251fcfcc805462ab6686a
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Sun Feb 10 13:36:17 2013 -0600

    \Fixed teleop to work with behaviors. Teleop currently has priority over behaviors

diff --git a/scout/motors/msg/set_motors.msg b/scout/motors/msg/set_motors.msg
index 5957eb2..d8da771 100644
--- a/scout/motors/msg/set_motors.msg
+++ b/scout/motors/msg/set_motors.msg
@@ -5,6 +5,7 @@ bool fl_set
 bool fr_set
 bool bl_set
 bool br_set
+bool teleop_ON
 
 # The absolute motor speeds to set (-128 to 127)
 # TODO check that this is the correct range
diff --git a/scout/scoutsim/src/old_scout_teleop.cpp b/scout/scoutsim/src/old_scout_teleop.cpp
deleted file mode 100644
index 6e69665..0000000
--- a/scout/scoutsim/src/old_scout_teleop.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-#include <ros/ros.h>
-#include <motors/set_motors.h>
-#include <signal.h>
-#include <termios.h>
-#include <stdio.h>
-
-// TODO: add this to a constants.h
-#define QUEUE_SIZE 10
-
-#define KEYCODE_R 0x43 
-#define KEYCODE_L 0x44
-#define KEYCODE_U 0x41
-#define KEYCODE_D 0x42
-#define KEYCODE_Q 0x71
-#define KEYCODE_S 0x73
-
-class TeleopScout
-{
-public:
-    TeleopScout();
-    void keyLoop();
-
-private:
-    ros::NodeHandle nh_;
-    bool fl_set_, fr_set_, bl_set_, br_set_;
-    int fl_speed_, fr_speed_, bl_speed_, br_speed_;
-    char last_key;
-    ros::Publisher motors_pub_;
-};
-
-TeleopScout::TeleopScout()
-{
-    fl_set_ = fr_set_ = bl_set_ = br_set_ = true;
-    fl_speed_ = fr_speed_ = bl_speed_ = br_speed_ = 0;
-    motors_pub_ = nh_.advertise<motors::set_motors>(
-                    "scout1/set_motors", QUEUE_SIZE);
-}
-
-int kfd = 0;
-struct termios cooked, raw;
-
-void quit(int sig)
-{
-    tcsetattr(kfd, TCSANOW, &cooked);
-    ros::shutdown();
-    exit(0);
-}
-
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "teleop_scout");
-  TeleopScout teleop_scout;
-
-  signal(SIGINT,quit);
-
-  teleop_scout.keyLoop();
-  
-  return(0);
-}
-
-
-void TeleopScout::keyLoop()
-{
-  char c;
-  bool dirty=false;
-
-  // get the console in raw mode                                                              
-  tcgetattr(kfd, &cooked);
-  memcpy(&raw, &cooked, sizeof(struct termios));
-  raw.c_lflag &=~ (ICANON | ECHO);
-  // Setting a new line, then end of file                         
-  raw.c_cc[VEOL] = 1;
-  raw.c_cc[VEOF] = 2;
-  tcsetattr(kfd, TCSANOW, &raw);
-
-  puts("Reading from keyboard");
-  puts("---------------------------");
-  puts("Use arrow keys to move the scout.");
-
-
-  for(;;)
-  {
-    ROS_INFO("loop pass engaged");
-    // get the next event from the keyboard  
-    if(read(kfd, &c, 1) < 0)
-    {
-      perror("read():");
-      exit(-1);
-    }
-
-    //ROS_DEBUG("value: 0x%02X\n", c);
-    //ROS_INFO("value: 0x%02X\n", c);
-    switch(c)
-    {
-      case KEYCODE_L:
-        ROS_INFO("LEFT");
-        fl_speed_ = bl_speed_ = 200;
-        fr_speed_ = br_speed_ = -200;
-        dirty = true;
-        break;
-      case KEYCODE_R:
-        ROS_INFO("RIGHT");
-        fl_speed_ = bl_speed_ = -200;
-        fr_speed_ = br_speed_ = 200;
-        dirty = true;
-        break;
-      case KEYCODE_U:
-        ROS_INFO("UP");
-        fl_speed_ = bl_speed_ = 200;
-        fr_speed_ = br_speed_ = 200;
-        dirty = true;
-        break;
-      case KEYCODE_D:
-        ROS_INFO("DOWN");
-        fl_speed_ = bl_speed_ = -200;
-        fr_speed_ = br_speed_ = -200;
-        dirty = true;
-        break;
-      default:
-        ROS_INFO("test");
-        fl_speed_ = bl_speed_ = 0;
-        fr_speed_ = br_speed_ = 0;
-        dirty = true;
-        break;
-    }
-   
-    motors::set_motors msg;
-    msg.fl_set = fl_set_;
-    msg.fr_set = fr_set_;
-    msg.bl_set = bl_set_;
-    msg.br_set = br_set_;
-    msg.fl_speed = fl_speed_;
-    msg.fr_speed = fr_speed_;
-    msg.bl_speed = bl_speed_;
-    msg.br_speed = br_speed_;
-    
-    if(dirty == true)
-    {
-      if (last_key != c)
-      {
-        motors_pub_.publish(msg);
-      }
-      dirty=false;
-    }
-    last_key = c;
-  }
-
-  return;
-}
-
-
-
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 92f7846..b97f91e 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -65,6 +65,8 @@ namespace scoutsim
         : path_bitmap(path_bitmap)
           , sonar_visual_on(false)
           , sonar_on(true)
+          , ignore_behavior(false)
+          , current_teleop_scout("")
           , node (nh)
           , scout_image(scout_image)
           , pos(pos)
@@ -134,22 +136,27 @@ namespace scoutsim
     {
         last_command_time = ros::WallTime::now();
 
-        if(msg->fl_set)
+        //ignore non-teleop commands if commands if teleop is ON
+        if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
         {
-            motor_fl_speed = absolute_to_mps(msg->fl_speed);
-        }
-        if(msg->fr_set)
-        {
-            motor_fr_speed = absolute_to_mps(msg->fr_speed);
-        }
-        if(msg->bl_set)
-        {
-            motor_bl_speed = absolute_to_mps(msg->bl_speed);
-        }
-        if(msg->br_set)
-        {
-            motor_br_speed = absolute_to_mps(msg->br_speed);
+            if(msg->fl_set)
+            {
+                motor_fl_speed = absolute_to_mps(msg->fl_speed);
+            }
+            if(msg->fr_set)
+            {
+                motor_fr_speed = absolute_to_mps(msg->fr_speed);
+            }
+            if(msg->bl_set)
+            {
+                motor_bl_speed = absolute_to_mps(msg->bl_speed);
+            }
+            if(msg->br_set)
+            {
+                motor_br_speed = absolute_to_mps(msg->br_speed);
+            }
         }
+
     }
 
     bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
@@ -397,7 +404,8 @@ namespace scoutsim
                                         const wxImage& walls_image,
                                         wxColour background_color,
                                         wxColour sonar_color,
-                                        world_state state)
+                                        world_state state, 
+                                        string teleop_scoutname)
     {
         // Assume that the two motors on the same side will be set to
         // roughly the same speed. Does not account for slip conditions
@@ -409,6 +417,11 @@ namespace scoutsim
         float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
         float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed);
 
+        //store currently teleop'd scoutname
+        std::stringstream ss;
+        ss << "/" << teleop_scoutname;
+        current_teleop_scout = ss.str();
+
         Vector2 old_pos = pos;
 
         // Update encoders
@@ -439,6 +452,7 @@ namespace scoutsim
         int canvas_x = pos.x * PIX_PER_METER;
         int canvas_y = pos.y * PIX_PER_METER;
 
+
         {
             wxImage rotated_image =
                 scout_image.Rotate(orient - PI/2.0,
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index 605b067..c281330 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -118,7 +118,7 @@ namespace scoutsim
                                          const wxImage& walls_image,
                                          wxColour background_color,
                                          wxColour sonar_color,
-                                         world_state state);
+                                         world_state state,std::string teleop_scoutname);
             void paint(wxDC& dc);
             void set_sonar_visual(bool on);
 
@@ -155,6 +155,9 @@ namespace scoutsim
 	        wxBitmap *path_bitmap;
 	        bool sonar_visual_on;
             bool sonar_on;
+            bool ignore_behavior;
+            
+            std::string current_teleop_scout;
 
             ros::NodeHandle node;
 
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 2e9b0b4..3ea810d 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -130,6 +130,8 @@ namespace scoutsim
         teleop_type = TELEOP_OFF;
         teleop_l_speed = 0;
         teleop_r_speed = 0;
+        teleop_scoutname = "scout1";
+
         teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
 
         ROS_INFO("Starting scoutsim with node name %s",
@@ -240,6 +242,8 @@ namespace scoutsim
         std::stringstream ss;
         ss << "/" << req.scout_name << "/set_motors";
         teleop_pub = nh.advertise<motors::set_motors>(ss.str(), 1000);
+        teleop_scoutname = req.scout_name.c_str();
+        ROS_INFO("Teleoping %s...",teleop_scoutname.c_str()); //debug statement
 
         return true;
     }
@@ -533,6 +537,7 @@ namespace scoutsim
         msg.fr_set = true;
         msg.bl_set = true;
         msg.br_set = true;
+        msg.teleop_ON = true;
 
         msg.fl_speed = teleop_l_speed;
         msg.fr_speed = teleop_r_speed;
@@ -544,6 +549,8 @@ namespace scoutsim
 
     void SimFrame::updateScouts()
     {
+        std::string teleop_out = "";
+
         if (last_scout_update.isZero())
         {
             last_scout_update = ros::WallTime::now();
@@ -562,12 +569,19 @@ namespace scoutsim
 
         for (; it != end; ++it)
         {
+
+        //default to scout1
+        if (teleop_type != TELEOP_OFF)
+        {
+            teleop_out = teleop_scoutname;
+        }
+
             it->second->update(SIM_TIME_REFRESH_RATE,
                                path_dc, sonar_dc,
                                path_image, lines_image, walls_image,
                                path_dc.GetBackground().GetColour(),
                                sonar_dc.GetBackground().GetColour(),
-                               state);
+                               state,teleop_out);
         }
 
         for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
diff --git a/scout/scoutsim/src/sim_frame.h b/scout/scoutsim/src/sim_frame.h
index 7163a0d..ccb5a7b 100644
--- a/scout/scoutsim/src/sim_frame.h
+++ b/scout/scoutsim/src/sim_frame.h
@@ -148,6 +148,7 @@ namespace scoutsim
             short teleop_l_speed;
             short teleop_r_speed;
             ros::Publisher teleop_pub;
+            std::string teleop_scoutname;
             int teleop_type;
 
             int teleop_fluid_speed;
