commit ddfeb11183e90127649a5f69425bedc926bda71c
Author: Priya <pdeo@Phlegethon.(none)>
Date:   Sat Nov 17 02:15:03 2012 -0500

    Fixing bugs with ghost scout (these were problems in odometry related
    to constants in the simulator and coordinate frame and theta representation
    in the simulator). Also made ghost scout translucent in order to differentiate
    it.

diff --git a/scout/libscout/src/behaviors/Odometry.cpp b/scout/libscout/src/behaviors/Odometry.cpp
index 1fc7ded..930f7fa 100644
--- a/scout/libscout/src/behaviors/Odometry.cpp
+++ b/scout/libscout/src/behaviors/Odometry.cpp
@@ -23,10 +23,6 @@ void Odometry::get_position()
   motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
   motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
 
-  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);
-  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
-  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
-
   // Get Left and Right distance
   left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
                              ((float)(motor_bl_dist))/2.0);
@@ -34,12 +30,14 @@ void Odometry::get_position()
                               ((float)(motor_br_dist))/2.0);
 
   total_dist = (left_dist)/2.0 + (right_dist)/2.0;
-  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);
-
-  ROS_INFO("l: %f   r: %f   theta: %f", left_dist, right_dist, theta);
-
-  scout_pos->x += total_dist*sin(theta);
-  scout_pos->y += total_dist*cos(theta);
+  theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
+
+  //Use negative theta because we measure theta from 0 on the
+  //right to 180 on the left counter-clock-wise, but this is
+  //negative theta in the coordinate frame.
+  //Also, subtract the delta from y because positive y is down.
+  scout_pos->x += total_dist*cos(-theta);
+  scout_pos->y -= total_dist*sin(-theta);
   scout_pos->theta = fmod(theta, 2*M_PI);
 
   //Save state for next time in.
@@ -65,7 +63,5 @@ void Odometry::run()
     position.y = scout_pos->y;
     position.theta = scout_pos->theta;
     scout_position.publish(position);
-
-    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
   }
 }
diff --git a/scout/libscout/src/behaviors/Odometry.h b/scout/libscout/src/behaviors/Odometry.h
index 5601dcf..dbcf8f9 100644
--- a/scout/libscout/src/behaviors/Odometry.h
+++ b/scout/libscout/src/behaviors/Odometry.h
@@ -6,10 +6,10 @@
 #include "messages/ScoutPosition.h"
 
 
-#define WHEEL_RADIUS  2
+#define WHEEL_RADIUS  .2
 #define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
-#define WHEEL_BASE    5
-#define ENCODER_COUNT (4*1200*M_PI/5)
+#define WHEEL_BASE    1.2
+#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
 #define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
 
 typedef struct{
diff --git a/scout/scoutsim/src/ghost_scout.cpp b/scout/scoutsim/src/ghost_scout.cpp
index 84cfbd1..1056682 100644
--- a/scout/scoutsim/src/ghost_scout.cpp
+++ b/scout/scoutsim/src/ghost_scout.cpp
@@ -76,8 +76,6 @@ namespace scoutsim
 
   void GhostScout::setPosition(const ::messages::ScoutPosition& msg)
   {
-    ROS_INFO("UPDATING GHOST SCOUT POSITION");
-
     pos.x = start_x + msg.x;
     pos.y = start_y - msg.y;
     orient = msg.theta;
@@ -104,12 +102,36 @@ namespace scoutsim
       world_state state)
   {
 
+    wxImage rotated_image = scout_image.Rotate(orient - M_PI/2.0,
+        wxPoint(scout_image.GetWidth() / 2,
+          scout_image.GetHeight() / 2),
+        false);
+
+    for (int y = 0; y < rotated_image.GetHeight(); ++y)
+    {
+      for (int x = 0; x < rotated_image.GetWidth(); ++x)
+      {
+        if (rotated_image.GetRed(x, y) == 255
+            && rotated_image.GetBlue(x, y) == 255
+            && rotated_image.GetGreen(x, y) == 255)
+        {
+          rotated_image.SetAlpha(x, y, 0);
+        }
+        else
+        {
+          rotated_image.SetAlpha(x, y, 125);
+        }
+      }
+    }
+
+    scout = wxBitmap(rotated_image);
+
     geometry_msgs::Pose2D my_pose;
     my_pose.x = pos.x;
     my_pose.y = pos.y;
     my_pose.theta = orient;
 
-    //ROS_INFO("Ghost scout is at position %f  %f  %f", pos.x, pos.y, orient);
+    ROS_INFO("Scout position %f  %f  %f", pos.x, pos.y, orient);
 
     return my_pose;
   }
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index b46f909..1d9ecf7 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -465,7 +465,7 @@ bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
 
         Pose p;
         p.x = pos.x;
-        p.y = state.canvas_height - pos.y;
+        p.y = pos.y;
         p.theta = orient;
         p.linear_velocity = l_speed;
         p.angular_velocity = r_speed;
diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index 1e00931..6c5a596 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -122,7 +122,7 @@ namespace scoutsim
             &SimFrame::wirelessCallback, this);
 
         // Teleop
-        teleop_type = TELEOP_OFF;
+        teleop_type = TELEOP_PRECISE;
         teleop_l_speed = 0;
         teleop_r_speed = 0;
         teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
