From c7da3d81dd7fa4174c3257b4408467a41a90e66a Mon Sep 17 00:00:00 2001
From: Steve McGuire <stephen.mcguire@colorado.edu>
Date: Wed, 11 Feb 2015 16:28:02 -0700
Subject: [PATCH] Added support for publishing raw wheel encoder ticks

---
 include/husky_base/husky_hardware.h |  9 +++++++++
 src/husky_base.cpp                  |  2 ++
 src/husky_hardware.cpp              | 31 +++++++++++++++++++++++++++++
 3 files changed, 42 insertions(+)

diff --git a/include/husky_base/husky_hardware.h b/include/husky_base/husky_hardware.h
index 0550aa4..0a34a26 100644
--- a/include/husky_base/husky_hardware.h
+++ b/include/husky_base/husky_hardware.h
@@ -40,6 +40,8 @@
 #include "ros/ros.h"
 #include "sensor_msgs/JointState.h"
 #include "husky_msgs/HuskyStatus.h"
+#include "husky_msgs/HuskyWheelTick.h"
+
 #include <string>
 
 namespace husky_base
@@ -54,6 +56,7 @@ namespace husky_base
     HuskyHardware(ros::NodeHandle nh, ros::NodeHandle private_nh);
 
     void updateJointsFromHardware();
+    void updateWheelTicksFromHardware();
 
     void writeCommandsToHardware();
 
@@ -86,11 +89,17 @@ namespace husky_base
     ros::Publisher diagnostic_publisher_;
     husky_msgs::HuskyStatus husky_status_msg_;
     diagnostic_updater::Updater diagnostic_updater_;
+
     HuskyHardwareDiagnosticTask<clearpath::DataSystemStatus> system_status_task_;
     HuskyHardwareDiagnosticTask<clearpath::DataPowerSystem> power_status_task_;
     HuskyHardwareDiagnosticTask<clearpath::DataSafetySystemStatus> safety_status_task_;
     HuskySoftwareDiagnosticTask software_status_task_;
 
+    //Wheel tick support
+    ros::Publisher wheel_publisher_;
+    husky_msgs::HuskyWheelTick husky_wheel_msg_;
+    void initializeWheelTicks();
+
     // ROS Parameters
     double wheel_diameter_, max_accel_, max_speed_;
 
diff --git a/src/husky_base.cpp b/src/husky_base.cpp
index 5b7abfe..34a3e7f 100644
--- a/src/husky_base.cpp
+++ b/src/husky_base.cpp
@@ -41,6 +41,8 @@ void controlLoop(const ros::TimerEvent &event, husky_base::HuskyHardware &husky,
 {
   husky.reportLoopFrequency(event);
   husky.updateJointsFromHardware();
+  husky.updateWheelTicksFromHardware();
+
   cm.update(event.current_real, event.current_real - event.last_real);
   husky.writeCommandsToHardware();
 }
diff --git a/src/husky_hardware.cpp b/src/husky_hardware.cpp
index f8245ec..440a1df 100644
--- a/src/husky_hardware.cpp
+++ b/src/husky_hardware.cpp
@@ -63,6 +63,7 @@ namespace husky_base
     connect(port);
     resetTravelOffset();
     initializeDiagnostics();
+    initializeWheelTicks();
     registerControlInterfaces();
   }
 
@@ -128,6 +129,10 @@ namespace husky_base
     diagnostic_publisher_ = nh_.advertise<husky_msgs::HuskyStatus>("status", 10);
   }
 
+  void HuskyHardware::initializeWheelTicks()
+  {
+        wheel_publisher_ = nh_.advertise<husky_msgs::HuskyWheelTick>("wheel_tick", 10);
+  }
 
   /**
   * Register interfaces with the RobotHW interface manager, allowing ros_control operation
@@ -160,11 +165,37 @@ namespace husky_base
     diagnostic_publisher_.publish(husky_status_msg_);
   }
 
+  /*Update the wheel ticks and publish */
+  void HuskyHardware::updateWheelTicksFromHardware()
+  {
+    Msg<clearpath::DataEncodersRaw>::Ptr enc = Msg<clearpath::DataEncodersRaw>::getUpdate();
+    
+    int index = 0;
+    if (enc)
+      {
+	husky_wheel_msg_.name.clear();
+	husky_wheel_msg_.tickCount.clear();
+
+	//ROS_INFO("Found %d encoders", enc->getCount());
+	husky_wheel_msg_.name.push_back("left");
+        husky_wheel_msg_.tickCount.push_back(enc->getTicks(index));
+
+	index++;
+	husky_wheel_msg_.name.push_back("right");
+        husky_wheel_msg_.tickCount.push_back(enc->getTicks(index));
+	
+	husky_wheel_msg_.header.stamp = ros::Time::now();
+	wheel_publisher_.publish(husky_wheel_msg_);
+      }
+  }
+
   /**
   * Pull latest speed and travel measurements from MCU, and store in joint structure for ros_control
   */
   void HuskyHardware::updateJointsFromHardware()
   {
+
+    
     Msg<clearpath::DataEncoders>::Ptr enc = Msg<clearpath::DataEncoders>::getUpdate();
     if (enc)
     {