need odometry insight for two wheel robot [closed]

edit

Updated my odometry: new code follows: Still robot not following trajectory. Not sure why map is following base_link? odom stays fixed.

#include <string>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <control_msgs/JointControllerState.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/JointState.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>


using namespace sensor_msgs;
using namespace message_filters;

class PublishOdometry
{
public:
  PublishOdometry()
  {
     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, &PublishOdometry::handelerInitialPose,this);
  }


void handelerInitialPose(geometry_msgs::PoseWithCovarianceStamped msg)
{
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg.header.stamp;
     odom_trans.transform.translation.x = msg.pose.pose.position.x;
     odom_trans.transform.translation.y = msg.pose.pose.position.y;
     odom_trans.transform.translation.z = msg.pose.pose.position.z;
     odom_trans.transform.rotation = msg.pose.pose.orientation;

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = msg.header.stamp;
     odometry.pose = msg.pose;

     // set starting point
     theta = msg.pose.pose.orientation.z;
     x = msg.pose.pose.position.x;
     y = msg.pose.pose.position.y;

     // publish initial postion
     odom_broadcaster.sendTransform(odom_trans);
     pubOdometry.publish(odometry);

     // start sync'd joint state callbacks 
     j1_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint1_position_controller/state", 10));
     j2_sub.reset(new Subscriber<control_msgs::JointControllerState>(n, "/rrbot/joint2_position_controller/state", 10));
     sync.reset(new TimeSynchronizer<control_msgs::JointControllerState, control_msgs::JointControllerState>(*j1_sub, *j2_sub, 10));
     sync->registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
}


void handelerOdometry(const control_msgs::JointControllerStateConstPtr& msg1, const control_msgs::JointControllerStateConstPtr& msg2)
{

// based on: <a href="http://rossum.sourceforge.net/papers/DiffSteer/">http://rossum.sourceforge.net/papers/DiffSteer/</a>

     double wheel_track = .21;   //distance between two wheels in meters  : in urdf dist is .3 in sdf it is .16 wheels are .05

     float   j1 = msg1->process_value;   // values are velocities: using velocity controller:  meters/sec
     float   j2 = msg2->process_value;   

     if (msg1->set_point == 0) j1 =0;
     if (msg2->set_point == 0) j2 =0;

     double  dt = msg1->time_step;                        // elapsed time in seconds
     float   dx     =   ( ((j1 + j2)*dt ) / 2.0);         // distance in meters
     float   dtheta =   ( ((j1 - j2)*dt ) / wheel_track);  // rotation

     if (abs(dtheta)>.001) 
     {
        x = x + (dx/dtheta)*(dx*sin(theta + dtheta)-sin(theta));                                    
        y = y - (dx/dtheta)*(dx*cos(theta + dtheta)-cos(theta));  
        theta = theta + dtheta;                                   
        theta = fmodf(theta + M_PI, 2*M_PI) - M_PI ; 
     } 
     else // straight line dtheta == 0
     {
        x = x - dx*cos(theta);
        y = y + dx*sin(theta);
     }



//**************************************************************

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = -sin(theta);           
     quaternion.w = cos(theta);  

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  msg1->header.stamp;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = y;
     odom_trans.transform.translation.z = 0;
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);


     ROS_INFO("pv1=[%f] pv2=[%f]",msg1->process_value, msg2->process_value);
     ROS_INFO("er1=[%f] er2=[%f] ",msg1->error, msg2->error);
     ROS_INFO("sp1=[%f] sp2=[%f] ",msg1->set_point, msg2->set_point ...