Add encoder data publish

CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation roscpp nav_msgs geome
 add_message_files(
   FILES
   BumperState.msg
+  Encoders.msg
 )
 #uncomment if you have defined services
 #add_service_files(

RosAria.cpp

@@ -20,6 +20,7 @@
 #include "tf/transform_datatypes.h"
 #include <dynamic_reconfigure/server.h>
 #include <rosaria/RosAriaConfig.h>
+#include "rosaria/Encoders.h"
 #include "std_msgs/Float64.h"
 #include "std_msgs/Float32.h"
 #include "std_msgs/Int8.h"
@@ -133,6 +134,9 @@ class RosAriaNode
 
     // whether to publish aria lasers
     bool publish_aria_lasers;
+
+    ros::Publisher encoder_pub;
+    rosaria::Encoders encoders;
 };
 
 void RosAriaNode::readParameters()
@@ -337,11 +341,15 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
   disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
   
   veltime = ros::Time::now();
+
+  encoder_pub = n.advertise<rosaria::Encoders>("encoders",1);
 }
 
 RosAriaNode::~RosAriaNode()
 {
   // disable motors and sonar.
+  robot->stopEncoderPackets();
+
   robot->disableMotors();
   robot->disableSonar();
 
@@ -520,6 +528,8 @@ int RosAriaNode::Setup()
   if (cmdvel_timeout_param > 0.0)
     cmdvel_watchdog_timer = n.createTimer(ros::Duration(0.1), &RosAriaNode::cmdvel_watchdog, this);
 
+  robot->requestEncoderPackets();
+
   ROS_INFO_NAMED("rosaria", "rosaria: Setup complete");
   return 0;
 }
@@ -690,6 +700,12 @@ void RosAriaNode::publish()
       sonar_pub.publish(cloud);
     }
   } // end if sonar_enabled
+  // send the encoder information
+  encoders.left_encoder=robot->getLeftEncoder();
+  encoders.right_encoder=robot->getRightEncoder();
+  encoders.header.stamp=ros::Time::now();
+  encoders.header.frame_id=frame_id_odom;
+  encoder_pub.publish(encoders);
 }
 
 bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)

msg/Encoders.msg

+Header header
+int64 left_encoder
+int64 right_encoder