commit 9f547ef719e5b32f52210cdb56bc7cae9eb16c7d
Author: Alex Zirbel <azirbel@andrew.cmu.edu>
Date:   Sat Jan 28 16:58:25 2012 -0500

    Added scoutsim support for encoders.
    
    To see the query_encoders.srv (used for accessing this data) in action, use: rosrun libscout test_encoders. You will see a stream of encoder data.

diff --git a/scout/encoders/srv/query_encoders.srv b/scout/encoders/srv/query_encoders.srv
index 92893c9..088ab4d 100644
--- a/scout/encoders/srv/query_encoders.srv
+++ b/scout/encoders/srv/query_encoders.srv
@@ -1,6 +1,6 @@
 ---
 # All units are simply in encoder ticks since encoders were turned on.
-int8 fl_distance
-int8 fr_distance
-int8 bl_distance
-int8 br_distance
+uint32 fl_distance
+uint32 fr_distance
+uint32 bl_distance
+uint32 br_distance
diff --git a/scout/libscout/CMakeLists.txt b/scout/libscout/CMakeLists.txt
index 9257467..5f4ff14 100644
--- a/scout/libscout/CMakeLists.txt
+++ b/scout/libscout/CMakeLists.txt
@@ -30,3 +30,4 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #target_link_libraries(example ${PROJECT_NAME})
 
 rosbuild_add_executable(libscout src/Behavior.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/CliffsensorControl.cpp)
+rosbuild_add_executable(test_encoders src/test_encoders.cpp)
diff --git a/scout/libscout/manifest.xml b/scout/libscout/manifest.xml
index 1525ce4..bc4fc7b 100644
--- a/scout/libscout/manifest.xml
+++ b/scout/libscout/manifest.xml
@@ -11,6 +11,7 @@
   <depend package="std_msgs"/>
   <depend package="roscpp"/>
   <depend package="motors"/>
+  <depend package="encoders"/>
   <depend package="headlights"/>
   <depend package="buttons"/>
   <depend package="sonar"/>
diff --git a/scout/libscout/src/test_encoders.cpp b/scout/libscout/src/test_encoders.cpp
new file mode 100644
index 0000000..2c15771
--- /dev/null
+++ b/scout/libscout/src/test_encoders.cpp
@@ -0,0 +1,36 @@
+
+#include <ros/ros.h>
+#include <encoders/query_encoders.h>
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "test_encoders");
+    ros::NodeHandle node;
+
+    ros::ServiceClient query_encoders_client =
+        node.serviceClient<encoders::query_encoders>("/scout1/query_encoders");
+
+    ros::Rate r(10);
+
+    while (ros::ok())
+    {
+        encoders::query_encoders srv;
+        if (query_encoders_client.call(srv))
+        {
+            printf("Got encoders: fl:%u, fr:%u, bl:%u, br:%u).\n",
+                srv.response.fl_distance,
+                srv.response.fr_distance,
+                srv.response.bl_distance,
+                srv.response.br_distance);
+        }
+        else
+        {
+            printf("ERROR: Failed to call service.\n");
+        }
+
+        ros::spinOnce();
+        r.sleep();
+    }
+
+    return 0;
+}
diff --git a/scout/scoutsim/manifest.xml b/scout/scoutsim/manifest.xml
index bb90fbe..95e8318 100644
--- a/scout/scoutsim/manifest.xml
+++ b/scout/scoutsim/manifest.xml
@@ -14,6 +14,7 @@
   <depend package="rosconsole"/>
   <depend package="std_srvs"/>
   <depend package="motors" />
+  <depend package="encoders" />
   
   <rosdep name="wxwidgets"/>
 
@@ -34,6 +35,7 @@
     <depend package="rosconsole"/>
     <depend package="std_srvs"/>
     <depend package="motors" />
+    <depend package="encoders" />
 
     <msgs>
       msg/Color.msg  msg/Pose.msg  msg/Velocity.msg 
diff --git a/scout/scoutsim/src/scout.cpp b/scout/scoutsim/src/scout.cpp
index 6983f8c..b5536b0 100644
--- a/scout/scoutsim/src/scout.cpp
+++ b/scout/scoutsim/src/scout.cpp
@@ -56,15 +56,21 @@
 namespace scoutsim
 {
     Scout::Scout(const ros::NodeHandle& nh,
-            const wxImage& scout_image,
-            const Vector2& pos,
-            float orient)
+                 const wxImage& scout_image,
+                 const Vector2& pos,
+                 float orient)
         : node (nh)
           , scout_image(scout_image)
           , pos(pos)
           , orient(orient)
-          , lin_vel(0.0)
-          , ang_vel(0.0)
+          , motor_fl_speed(0)
+          , motor_fr_speed(0)
+          , motor_bl_speed(0)
+          , motor_br_speed(0)
+          , fl_ticks(0)
+          , fr_ticks(0)
+          , bl_ticks(0)
+          , br_ticks(0)
           , pen_on(true)
           , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
     {
@@ -78,6 +84,10 @@ namespace scoutsim
         set_pen_srv = node.advertiseService("set_pen",
                                             &Scout::setPenCallback,
                                             this);
+        query_encoders_srv =
+            node.advertiseService("query_encoders",
+                                  &Scout::query_encoders_callback,
+                                  this);
 
         meter = scout.GetHeight();
     }
@@ -108,15 +118,6 @@ namespace scoutsim
             motor_br_speed = msg->br_speed;
         }
 
-        // Assume that the two motors on the same side will be set to
-        // roughly the same speed. Does not account for slip conditions
-        // when they are set to different speeds.
-        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
-        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
-
-        // Set the linear and angular speeds
-        lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
-        ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
     }
 
     float Scout::getSonar(float angle)
@@ -144,12 +145,40 @@ namespace scoutsim
         return true;
     }
 
+    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
+                                        encoders::query_encoders::Response& res)
+    {
+        res.fl_distance = fl_ticks;
+        res.fr_distance = fr_ticks;
+        res.bl_distance = bl_ticks;
+        res.br_distance = br_ticks;
+
+        return true;
+    }
+
     void Scout::update(double dt, wxMemoryDC& path_dc,
                         const wxImage& path_image, wxColour background_color,
                         float canvas_width, float canvas_height)
     {
+        // Assume that the two motors on the same side will be set to
+        // roughly the same speed. Does not account for slip conditions
+        // when they are set to different speeds.
+        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
+        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
+
+        // Set the linear and angular speeds
+        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
+        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
+
         Vector2 old_pos = pos;
 
+        // Update encoders
+        /// @todo replace
+        fl_ticks += (unsigned int) motor_fl_speed;
+        fr_ticks += (unsigned int) motor_fr_speed;
+        bl_ticks += (unsigned int) motor_bl_speed;
+        br_ticks += (unsigned int) motor_br_speed;
+
         orient = fmod(orient + ang_vel * dt, 2*PI);
         pos.x += sin(orient + PI/2.0) * lin_vel * dt;
         pos.y += cos(orient + PI/2.0) * lin_vel * dt;
diff --git a/scout/scoutsim/src/scout.h b/scout/scoutsim/src/scout.h
index 69ac252..98ae629 100644
--- a/scout/scoutsim/src/scout.h
+++ b/scout/scoutsim/src/scout.h
@@ -52,6 +52,7 @@
 #include <boost/shared_ptr.hpp>
 
 #include <motors/set_motors.h>
+#include <encoders/query_encoders.h>
 
 #include <scoutsim/Pose.h>
 #include <scoutsim/SetPen.h>
@@ -110,6 +111,8 @@ namespace scoutsim
             float getSonar(float angle);
             bool setPenCallback(scoutsim::SetPen::Request&,
                                 scoutsim::SetPen::Response&);
+            bool query_encoders_callback(encoders::query_encoders::Request&,
+                                         encoders::query_encoders::Response&);
 
             ros::NodeHandle node;
 
@@ -119,17 +122,23 @@ namespace scoutsim
             Vector2 pos;
             float orient;
 
+            /// @todo should these be an array or something?
+
             // Keep track of the last commanded speeds sent to the sim
             int motor_fl_speed;
             int motor_fr_speed;
             int motor_bl_speed;
             int motor_br_speed;
 
+            // Keep track of encoder ticks for each motor
+            unsigned int fl_ticks;
+            unsigned int fr_ticks;
+            unsigned int bl_ticks;
+            unsigned int br_ticks;
+
             // Each scout has a unique id number, which is also displayed on its image.
             int scout_id;
 
-            float lin_vel;
-            float ang_vel;
             bool pen_on;
             wxPen pen;
 
@@ -137,6 +146,7 @@ namespace scoutsim
             ros::Publisher pose_pub;
             ros::Publisher color_pub;
             ros::ServiceServer set_pen_srv;
+            ros::ServiceServer query_encoders_srv;
 
             ros::WallTime last_command_time;
 
