commit f09d002e49f709a5570f26d99d61ed92580b17a7
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Sun Mar 3 14:06:18 2013 -0600

    behaviors now overwrite teleop (see comments in scout.cpp for details)

diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 0a9b644..b4b6414 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -66,7 +66,6 @@ namespace scoutsim
           , sonar_visual_on(false)
           , sonar_on(true)
           , ignore_behavior(false)
-          , current_teleop_scout("")
           , node (nh)
           , scout_image(scout_image)
           , pos(pos)
@@ -120,6 +119,9 @@ namespace scoutsim
         sonar_stop_r = 23;
         sonar_direction = 1;
         sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
+        
+        // Init latch
+        teleop_latch = 0;
     }
 
     float Scout::absolute_to_mps(int absolute_speed)
@@ -137,7 +139,17 @@ namespace scoutsim
         last_command_time = ros::WallTime::now();
 
         //ignore non-teleop commands if commands if teleop is ON
-        if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
+        //if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
+        //{
+
+        //latch value indicates number of uninterrupted teleop messages
+        //before teleop latch shifts again
+        if (!(msg->teleop_ON) && teleop_latch < 3)
+        {
+            teleop_latch++;
+        }
+
+        if (!(msg->teleop_ON) || teleop_latch ==0)
         {
             if(msg->fl_set)
             {
@@ -155,7 +167,16 @@ namespace scoutsim
             {
                 motor_br_speed = absolute_to_mps(msg->br_speed);
             }
+        }        
+
+        //if a teleop message comes through, decrease the latch
+        //latch code works on the assumption there will be more behavior messages
+        //than teleop messages
+        if (msg->teleop_ON && teleop_latch>0)
+        {
+            teleop_latch--;
         }
+        //}
 
     }
 
@@ -405,8 +426,7 @@ namespace scoutsim
                                         const wxImage& walls_image,
                                         wxColour background_color,
                                         wxColour sonar_color,
-                                        world_state state, 
-                                        string teleop_scoutname)
+                                        world_state state)
     {
         // Assume that the two motors on the same side will be set to
         // roughly the same speed. Does not account for slip conditions
@@ -419,9 +439,9 @@ namespace scoutsim
         float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH;
 
         //store currently teleop'd scoutname
-        std::stringstream ss;
-        ss << "/" << teleop_scoutname;
-        current_teleop_scout = ss.str();
+        //std::stringstream ss;
+        //ss << "/" << teleop_scoutname;
+        //current_teleop_scout = ss.str();
 
         Vector2 old_pos = pos;
 
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index c281330..b3fb90b 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,std::string teleop_scoutname);
+                                         world_state state);
             void paint(wxDC& dc);
             void set_sonar_visual(bool on);
 
@@ -151,13 +151,15 @@ namespace scoutsim
             int old_back_dx;
             int old_back_dy;
             bool isFront;
+            
+            int teleop_latch;
 
 	        wxBitmap *path_bitmap;
 	        bool sonar_visual_on;
             bool sonar_on;
             bool ignore_behavior;
             
-            std::string current_teleop_scout;
+            //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 3ea810d..d0a6c89 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -549,7 +549,6 @@ namespace scoutsim
 
     void SimFrame::updateScouts()
     {
-        std::string teleop_out = "";
 
         if (last_scout_update.isZero())
         {
@@ -570,18 +569,12 @@ 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,teleop_out);
+                               state);
         }
 
         for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
