rviz robot moving in place

edit

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 ...