odom code please help

edit

hello, I have my own robot platform and I'm trying to implement SLAM using ROS. I'm using Roboteq SDC21xx motor controller. Now I am trying to get correct odotmetry data manually, not using any drivers.

here is my code:

#include <iostream>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "roboteq_motor/RoboteqDevice.h"
#include "roboteq_motor/ErrorCodes.h"
#include "roboteq_motor/Constants.h"
#include <unistd.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <cmath>
# define M_PI  3.14159265358979323846  /* pi - which you should NOT be redefining since it's already available in cmath */ 

using namespace std;
int status;
RoboteqDevice device; // Motor Controller Roboteq API

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "odom"); 
    ros::NodeHandle nh; 
    string response = ""; 
    status = device.Connect("/dev/ttyACM0"); 
    if(status != RQ_SUCCESS) 
    {
        cout<<"Error connecting to device: "<<status<<"."<<endl;
        return 1;
    }

    double encoder1;
    double encoder2;
    int pre1;
    int pre2;
    double distance;

    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
    tf::TransformBroadcaster odom_broadcaster;

    double x = 0;
    double y = 0;
    double th = 0;
    ros::Rate r(20);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    while(nh.ok()){
        device.SetConfig(_EMOD, 1); //Mc settings
        device.SetConfig(_EMOD, 2); //Mc settings
        device.SetConfig(_EPPR, 1, 200);  //Mc settings
        device.SetConfig(_EPPR, 2, 200);  //Mc settings
        device.SetConfig(_MXPR, 1, 83);  //Mc settings
        device.SetConfig(_MXPF, 1, 83);  //Mc settings
        device.GetValue(_ABCNTR, 1, pre1); // get the counts of motor 1
        device.GetValue(_ABCNTR, 2, pre2); // get the counts of motor 2

        encoder1= (double)-pre1; // put pre1 integer value in a double variable and make it minus cuz when the robot is going fwd one encoder gives minus and the other positive
        encoder2= (double)pre2; // put pre2 value in a double variable since the method GetValue doesn't support doubles.

        encoder1 /= 982 * 0.8; // 982 is the number of pulses per revolution for motor 1 and 0.8 is the wheel's circumference in meters .
        encoder2 /= 966 * 0.8; // 966 is the number of pulses per revolution for motor 2 and 0.8 is the wheel's circumference in meters .

        distance= (encoder1 + encoder2) / 2; // distance
        th = ((encoder1- encoder2 ) / 0.36); //theta
        th=th*(M_PI/180); // convert the incoming angle to radian

        x = distance * cos(th); // x position
        y = distance * sin(th); // y position

        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

        odom_trans.header.stamp = current_time;
        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = 0.0;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat ;

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";

        //set the position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;
       odom.child_frame_id = "base_link";

        last_time = current_time;

        //send the transform
        odom_broadcaster ...