hello everyone ,

I'm trying to do gmapping ,i have two rotary encoders(left and right) while moving my robot moves in place in an anticlockwise direction in RVIZ checked for both encoders(L,R) odom spins on the same anti-clockwise direction

im getting almost same rpm on both side

here is my code

here is odom node:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>
#include <stdio.h>
#include <cmath>
#include <algorithm>
#include <robot_specs.h>


double rpm_act1 = 0.0;
double rpm_act2 = 0.0;
double rpm_req1 = 0.0;
double rpm_req2 = 0.0;
double gyro_x = 0.0;
double gyro_y = 0.0;
double gyro_z = 0.0;
double rpm_dt = 0.0;
double x_pos = 0.0;
double y_pos = 0.0;
double theta = 0.0;


ros::Time current_time;
ros::Time rpm_time(0.0);
ros::Time last_time(0.0);

void handle_rpm( const geometry_msgs::Vector3Stamped& rpm) {
  rpm_act1 = rpm.vector.x;
  rpm_act2 = rpm.vector.y;
  rpm_dt = rpm.vector.z;
  rpm_time = rpm.header.stamp;
}

void handle_gyro( const geometry_msgs::Vector3& gyro) {
  gyro_x = gyro.x;
  gyro_y = gyro.y;
  gyro_z = gyro.z;
}

int main(int argc, char** argv){
  ros::init(argc, argv, "base_controller");

  ros::NodeHandle n;
  ros::NodeHandle nh_private_("~");
  ros::Subscriber sub = n.subscribe("rpm", 50, handle_rpm);
  ros::Subscriber gyro_sub = n.subscribe("gyro", 50, handle_gyro);
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster broadcaster;

  double rate = 10.0;
  double linear_scale_positive = 1.0;
  double linear_scale_negative = 1.0;
  double angular_scale_positive = 1.0;
  double angular_scale_negative = 1.0;
  double angular_scale_accel = 1.0;
  double acc_theta = 0.0;
  double acc_x = 0.0;
  double acc_max_theta = 0.0;
  double acc_max_x = 0.0;
  double alpha = 0.0;
  bool publish_tf = false;//whether to publish tf or not. set to false if you want to use robot_pose_ekf
  bool use_imu = false;
  double dt = 0.0;
  double dx = 0.0;
  double dy = 0.0;
  double dth_odom = 0.0;
  double dth_gyro = 0.0;
  double dth = 0.0;
  double dth_prev = 0.0;
  double dth_curr = 0.0;
  double dxy_prev = 0.0;
  double dxy_ave = 0.0;
  double vx = 0.0;
  double vy = 0.0;
  double vth = 0.0;
  char base_link[] = "/base_link";
  char odom[] = "/odom";
  ros::Duration d(1.0);
  nh_private_.getParam("publish_rate", rate);
  nh_private_.getParam("publish_tf", publish_tf);
  nh_private_.getParam("linear_scale_positive", linear_scale_positive);
  nh_private_.getParam("linear_scale_negative", linear_scale_negative);
  nh_private_.getParam("angular_scale_positive", angular_scale_positive);
  nh_private_.getParam("angular_scale_negative", angular_scale_negative);
  nh_private_.getParam("angular_scale_accel", angular_scale_accel);
  nh_private_.getParam("alpha", alpha);
  nh_private_.getParam("use_imu", use_imu);

  ros::Rate r(rate);
  while(n.ok()){
    ros::spinOnce();
   // ros::topic::waitForMessage<geometry_msgs::Vector3Stamped>("rpm", n, d);
    current_time = ros::Time::now();
    dt =(current_time-last_time).toSec();
    dxy_ave = (rpm_act1+rpm_act2)*dt/(60*2);    //speed
    dth_odom = ((rpm_act2-rpm_act1)*dt)/(60*track_width);

    if(dxy_ave>0.0)
    {
        ROS_INFO("dxy_ave:%f, dth_odom:%f",dxy_ave,dth_odom);
    }

//    if (use_imu) dth_gyro = dt*gyro_z;



    dth = dth_odom;


    if(dth>0.0)
    {
        ROS_INFO("dth:%f",dth);
    }


    if (dth > 0) dth *= 5.0; //angular_scale_positive
    if (dth < 0) dth *= -5.0; //angular_scale_negative
    if (dxy_ave > 0) dxy_ave *= linear_scale_positive;
   // if (dxy_ave > 0) dxy_ave *= linear_scale_negative;

    dx = cos(dth) * dxy_ave;
    dy = -sin(dth) * dxy_ave;


    x_pos += (cos(theta) * dx - sin(theta) * dy);
    y_pos += (sin(theta) * dx + cos(theta) * dy);
    theta += dth;
    if(dx>0.0)
    {
        ROS_INFO("dx:%f,dy:%f",dx,dy);
        ROS_INFO("x_pos:%f,y_pos:%f, theta:%f",x_pos,y_pos,theta);
    }


//    if(theta >= two_pi) theta -= two_pi;
//    if(theta <= -two_pi) theta += two_pi;
 //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);

      geometry_msgs::TransformStamped t;
      t.header.stamp = current_time; //added 
      t.header.frame_id = odom;
      t.child_frame_id = base_link;
      t.transform.translation.x = x_pos;
      t.transform.translation.y = y_pos;
      t.transform.translation.z = 0.0;
      t.transform.rotation = odom_quat;
      broadcaster.sendTransform(t);

    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = current_time;
    odom_msg.header.frame_id = odom;
    odom_msg.pose.pose.position.x = x_pos;
    odom_msg.pose.pose.position.y = y_pos;
    odom_msg.pose.pose.position.z = 0.0;
    odom_msg.pose.pose.orientation = odom_quat;

/*    if (rpm_act1 == 0 && rpm_act2 == 0){
      odom_msg.pose.covariance[0] = 1e-9;
      odom_msg.pose.covariance[7] = 1e-3;
      odom_msg.pose.covariance[8] = 1e-9;
      odom_msg.pose.covariance[14] = 1e6;
      odom_msg.pose.covariance[21] = 1e6;
      odom_msg.pose.covariance[28] = 1e6;
      odom_msg.pose.covariance[35] = 1e-9;
      odom_msg.twist.covariance[0] = 1e-9;
      odom_msg.twist.covariance[7] = 1e-3;
      odom_msg.twist.covariance[8] = 1e-9;
      odom_msg.twist.covariance[14] = 1e6;
      odom_msg.twist.covariance[21] = 1e6;
      odom_msg.twist.covariance[28] = 1e6;
      odom_msg.twist.covariance[35] = 1e-9;
    }
    else{
      odom_msg.pose.covariance[0] = 1e-3;
      odom_msg.pose.covariance[7] = 1e-3;
      odom_msg.pose.covariance[8] = 0.0;
      odom_msg.pose.covariance[14] = 1e6;
      odom_msg.pose.covariance[21] = 1e6;
      odom_msg.pose.covariance[28] = 1e6;
      odom_msg.pose.covariance[35] = 1e3;
      odom_msg.twist.covariance[0] = 1e-3;
      odom_msg.twist.covariance[7] = 1e-3;
      odom_msg.twist.covariance[8] = 0.0;
      odom_msg.twist.covariance[14] = 1e6;
      odom_msg.twist.covariance[21] = 1e6;
      odom_msg.twist.covariance[28] = 1e6;
      odom_msg.twist.covariance[35] = 1e3;
    }
*/  
   //set the velocity

    vx = (dt == 0)?  0 : dxy_ave/dt;
    vth = (dt == 0)? 0 : dth/dt;
    odom_msg.child_frame_id = base_link;
    odom_msg.twist.twist.linear.x = vx;
    odom_msg.twist.twist.linear.y = 0.0;
    odom_msg.twist.twist.angular.z =dth;

   //publish the message
    odom_pub.publish(odom_msg);
    last_time = current_time;
    r.sleep();
  }
}

Arduino code :

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <ros.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Twist.h> //telop
#include <std_msgs/Int32.h> //sending encoder data
#include <ros/time.h> //to get current ruuning ros time  frame
#include <Encoder.h> // encoder 


#define RH_Directionf 26  //input 1
#define RH_Directionb 28  // 2
#define LH_Directionf 22  //in 1
#define LH_Directionb 24  //in 2
#define RH_Pwm 9  // motor 1 pwm
#define LH_Pwm 10 //pwm pin for motor 2
#define LOOPTIME        100   // PID loop time(ms) calling loop at fixed for pid 
#define SMOOTH      10  //not used

// variables to store the number of encoder pulses
// for each motor

Encoder myEnc1(2, 4);   //encoder declarations
Encoder myEnc2(3, 5);
long oldPosition1  = 0, newPosition1; //variable to store enc data from encoder library
long oldPosition2  = 0, newPosition2;
long d1r, d2r = -999, d1l, d2l = -999;


volatile double leftCount = 0; //not used
volatile double rightCount = 0;//n u
volatile double prev, now , start, time_delay = 0, counter = 0, prev_rotR, prev_rotL, cur_rotR, cur_rotL; //variables for pid ,getting rps and rpm
volatile double error_l = 0, last_error1_l = 0 ;
volatile double error_r = 0, last_error1_r = 0 ;
volatile double int_errorl;
volatile double int_errorr;

unsigned long lastMilli = 0;       // loop timing
unsigned long lastMilliPub = 0;
double Required_rpml = 0;
double Required_rpmr = 0;
double RPM_left = 0;
double RPM_right = 0;
double Required_rpml_smoothed = 0;
double Required_rpmr_smoothed = 0;
char direction1 ;
char direction2 ;
int PWM_val1 = 0;
int PWM_val2 = 0;
float Kp = 45;
float Kd = 20;
float Ki = 0;

    ros::NodeHandle nh;

void handle_cmd( const geometry_msgs::Twist& cmd_msg)
{
  double x = cmd_msg.linear.x;
  double z = cmd_msg.angular.z;
  if (z == 0) {     // go straight
    // convert m/s to rpm
    Required_rpml = x * 95.469296598;
    Required_rpmr = Required_rpml;
  }
  else if (x == 0) {
    // convert rad/s to rpm
    Required_rpmr = z * 33.90000294; ///z < .307
    Required_rpml = -Required_rpmr;
  }
  else {
    Required_rpml = (x * 95.469296598) - (z * 33.90000294);
    Required_rpmr = (x * 95.469296598) + (z * 33.90000294);
  }
  int_errorr = 0; //error for pid
  int_errorl = 0;
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", handle_cmd);
geometry_msgs::Vector3Stamped rpm_msg;
std_msgs::Int32 rot, enc1;
ros::Publisher rpm_pub("rpm", &rpm_msg);
ros::Publisher enc("encoder1", &enc1);

ros::Time current_time;
ros::Time last_time;

void setup()
{
  Required_rpml = 0;
  Required_rpmr = 0;
  RPM_left = 0;
  RPM_right = 0;
  PWM_val1 = 0;
  PWM_val2 = 0;
  nh.initNode();
  //nh.getHardware()->setBaud(57600);
  nh.subscribe(sub);
  nh.advertise(rpm_pub);
  nh.advertise(enc);

 // Serial.begin(115200);
  pinMode(RH_Directionf, OUTPUT);
  pinMode(RH_Directionb, OUTPUT);
  pinMode(LH_Directionf, OUTPUT);
  pinMode(LH_Directionb, OUTPUT);
  pinMode(RH_Pwm, OUTPUT);
  pinMode(LH_Pwm, OUTPUT);

  prev = 0;
}

void loop()
{
  newPosition1 = myEnc1.read()/4;
  if (newPosition1 != oldPosition1)
  {
    oldPosition1 = newPosition1;
    d1r = newPosition1;
  }
  enc1.data = d1r;
  enc.publish(&enc1);


  newPosition2 = myEnc2.read()/4;
  if (newPosition2 != oldPosition2)
  {
    oldPosition2 = newPosition2;
    d1l = newPosition2;
  }
  nh.spinOnce();

  unsigned long tim = millis(); //getting in current time in milli sec
  if (tim - lastMilli >= LOOPTIME)    //100ms loop
  { // enter tmed loop
    time_delay = tim - prev;

    if (d1r > d2r)
    {
      //      cur_rotR = float(d1r - d2r) / ((float(tim - lastMilli)/1000));
      cur_rotR = float(d1r - d2r) / 400.0;
     cur_rotR /= (float(tim - lastMilli) / 1000.0);
    }
    else
    {
      // cur_rotR = (float(d2r - d1r)) / (float(tim - lastMilli)/1000);
      cur_rotR = float(d2r - d1r) / 400.0;
      cur_rotR /= (float(tim - lastMilli) / 1000.0);

    }

    if (d1l > d2l)
    {
      //cur_rotL = (float(d1l - d2l)) / (float(tim - lastMilli)/1000);
      cur_rotL = float(d1l - d2l) / 400.0;
      cur_rotL /= (float(tim - lastMilli) / 1000.0);

    }
    else
    {
      //cur_rotL = (float(d2l - d1l)) / (float(tim - lastMilli)/1000);
      cur_rotL = float(d2l - d1l) / 400.0;
      cur_rotL /= (float(tim - lastMilli) / 1000.0);
    }

    if (cur_rotL > prev_rotL)
    {
      RPM_left = (cur_rotL - prev_rotL) * 60; //  ****

    }
    else
    {
      RPM_left = (prev_rotL - cur_rotL) * 60; //  ****
    }

    if (cur_rotR > prev_rotR) 
    {
      RPM_right = (cur_rotR - prev_rotR) * 60;
    }
    else
    {
      RPM_right = (prev_rotR - cur_rotR) * 60;
    }

    prev_rotR = cur_rotR;
    prev_rotL = cur_rotL;

    //Serial.print("cur_rotR:");
    //Serial.print(cur_rotR);

     //Serial.print("cur_rotL:");
    //Serial.print(cur_rotL);

    //Serial.print("\td:");
    //Serial.print(d1r-d2r);

    //Serial.print("\td:");
   // Serial.print(d1l-d2l);

    //Serial.print("\tprev_rotR:");
    //Serial.print("\tprev_rotL:");
    //Serial.print(prev_rotR);
    //Serial.print(prev_rotL);
   // Serial.print("\tRPM_right:");
     //Serial.print("\tRPM_left:");
    //Serial.println(RPM_right);
   // Serial.println(RPM_left);

    d2r = d1r;
    d2l = d1l;
    error_l = - RPM_left + Required_rpml;
    int_errorl += error_l;
    int Value_l =  Value_l + 255 * ((Kp * error_l + Kd * (error_l - last_error1_l)) + Ki * (int_errorl)) / (Kp * 20 + Kd * 40 );
    last_error1_l = error_l;
    PWM_val1 = Value_l;


    error_r = - RPM_right + Required_rpmr;
    int_errorr += error_r;
    int Value_r =  Value_r + 255 * ((Kp * error_r + Kd * (error_r - last_error1_r)) + Ki * (int_errorr)) / (Kp * 20 + Kd * 40);
    last_error1_r = error_r;
    PWM_val2 = Value_r;



    if (PWM_val1 > 0) direction1 = 'f';
    else if (PWM_val1 < 0) direction1 = 'b';
    if (Required_rpml == 0) direction1 = 's';
    if (PWM_val2 > 0) direction2 = 'f';
    else if (PWM_val2 < 0) direction2 = 'b';
    if (Required_rpmr == 0) direction2 = 's';

    PWM_val1 = abs(PWM_val1);
    PWM_val2 = abs(PWM_val2);

    if ( direction1 == 'f')
    {
      digitalWrite(LH_Directionf, HIGH);
      digitalWrite(LH_Directionb, LOW);
      analogWrite(LH_Pwm, PWM_val1);
    }
    else if ( direction1 == 'b')
    {
      digitalWrite(LH_Directionf, LOW);
      digitalWrite(LH_Directionb, HIGH);
      analogWrite(LH_Pwm, PWM_val1);

    }
    else if (direction1 == 's')
    {
      digitalWrite(LH_Directionf, LOW);
      digitalWrite(LH_Directionb, LOW);
      analogWrite(LH_Pwm, PWM_val1);
    }

    if ( direction2 == 'f')
    {
      digitalWrite(RH_Directionf, HIGH);
      digitalWrite(RH_Directionb, LOW);
      analogWrite(RH_Pwm, PWM_val2);
    }
    else if ( direction2 == 'b')
    {
      digitalWrite(RH_Directionf, LOW);
      digitalWrite(RH_Directionb, HIGH);
      analogWrite(RH_Pwm, PWM_val2);
    }
    else if (direction2 == 's')
    {
      digitalWrite(RH_Directionf, LOW);
      digitalWrite(RH_Directionb, LOW);
      analogWrite(RH_Pwm, PWM_val2);
    }

    publishRPM(tim - lastMilli);
    lastMilli = tim;

  }
  if (tim - lastMilliPub >= LOOPTIME)
  {
    lastMilliPub = tim;
  }
}

void publishRPM(unsigned long tim)
{
  rpm_msg.header.stamp = nh.now();
  rpm_msg.vector.x = -RPM_left;
  rpm_msg.vector.y = RPM_right;
  rpm_msg.vector.z = double(tim) / 1000;
  rpm_pub.publish(&rpm_msg);
  nh.spinOnce();
}