Added a TF broadcaster.

Implemented the odometry equations.

config/ana_odom.yaml

+encoder_res: 100
+gearbox_ratio: 38.3
+wheel_radius: 0.11
+publish_tf: True
+base_link_frame: ana/odom

include/ana_odom_alg_node.h

@@ -32,6 +32,7 @@
 #include <nav_msgs/Odometry.h>
 #include <rosaria/Encoders.h>
 
+#include <tf/transform_broadcaster.h>
 // [service client headers]
 
 // [action server client headers]
@@ -54,6 +55,7 @@ class AnaOdomAlgNode : public algorithm_base::IriBaseAlgorithm<AnaOdomAlgorithm>
     void encoders_mutex_enter(void);
     void encoders_mutex_exit(void);
 
+    tf::TransformBroadcaster odom_broadcaster;
     // [service attributes]
 
     // [client attributes]

launch/ana_odom.launch

+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="config_file" default="$(find iri_ana_odom)/config/ana_odom.yaml" />
+  <arg name="node_name" default="ana_odom" />
+  <arg name="output" default="log" />
+  <arg name="launch_prefix" default="" />
+
+  <node pkg="iri_ana_odom"
+        type="iri_ana_odom"
+        name="$(arg node_name)"
+        output="$(arg output)"
+        launch-prefix="$(arg launch_prefix)">
+     <rosparam file="$(arg config_file)" command="load" />
+  </node>
+</launch>

src/ana_odom_alg_node.cpp

@@ -53,19 +53,62 @@ void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
   static long int prev_right_enc=0;
   static ros::Time prev_time=ros::Time::now();
 
-  long int left_inc,right_inc;
+  long int inc_left,inc_right;
+  double disp_left,disp_right;
   ros::Duration dt;
+  double v_left,v_right,v_trans,v_rot;
+  static double x=0.0,y=0.0,theta=0.0;
+
+  geometry_msgs::TransformStamped odom_trans;
 
   ROS_INFO("AnaOdomAlgNode::encoders_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
   this->encoders_mutex_enter();
-  left_inc=msg->left_encoder-prev_left_encoder;
-  right_inc=msg->right_encoder-prev_right_encoder;
-  dt=msg->header.stamp-pres_time;
-
-  
+  inc_left=msg->left_encoder-prev_left_enc;
+  inc_right=msg->right_encoder-prev_right_enc;
+  dt=msg->header.stamp-prev_time;
+
+  disp_left=((double)inc_left)*2.0*3.14159*this->config_.wheel_radius/(this->config_.encoder_res*this->config_.gearbox_ratio);
+  disp_right=((double)inc_right)*2.0*3.14159*this->config_.wheel_radius/(this->config_.encoder_res*this->config_.gearbox_ratio);
+
+  v_left=disp_left/dt.toSec();
+  v_right=disp_right/dt.toSec();
+  v_trans=(v_left+v_right)/2.0;
+  v_rot=(v_left-v_right)/2.0;
+
+  theta+=(disp_left-disp_right)/4.0;
+  x+=((disp_left+disp_right)/2.0)*cos(theta);
+  y+=((disp_left+disp_right)/2.0)*sin(theta);
+  theta+=(disp_left-disp_right)/4.0;
+
+  if(this->config_.publish_tf)
+  {
+    odom_trans.header.frame_id=msg->header.frame_id;
+    odom_trans.header.stamp=msg->header.stamp;
+    odom_trans.child_frame_id = this->config_.base_link_frame;
+    odom_trans.transform.translation.x = x;
+    odom_trans.transform.translation.y = y;
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation=tf::createQuaternionMsgFromYaw(theta);
+    this->odom_broadcaster.sendTransform(odom_trans);
+  }
+
+  odom_Odometry_msg_.header.stamp=msg->header.stamp;
+  odom_Odometry_msg_.header.frame_id=msg->header.frame_id;
+  odom_Odometry_msg_.child_frame_id = this->config_.base_link_frame;
+  odom_Odometry_msg_.pose.pose.position.x=x;
+  odom_Odometry_msg_.pose.pose.position.y=y;
+  odom_Odometry_msg_.pose.pose.position.z=0.0;
+  odom_Odometry_msg_.pose.pose.orientation=tf::createQuaternionMsgFromYaw(theta);
+  odom_Odometry_msg_.twist.twist.linear.x=v_trans;
+  odom_Odometry_msg_.twist.twist.linear.y=0.0;
+  odom_Odometry_msg_.twist.twist.linear.z=0.0;
+  odom_Odometry_msg_.twist.twist.angular.x=0.0;
+  odom_Odometry_msg_.twist.twist.angular.y=0.0;
+  odom_Odometry_msg_.twist.twist.angular.z=v_rot;
+  odom_publisher_.publish(odom_Odometry_msg_);
 
   prev_left_enc=msg->left_encoder;
   prev_right_enc=msg->right_encoder;