Publish odometry data based on encoders

edit

Hi to all,

I would like to publish odometry data starting by the encoders ticks as much accurately as possible. I've built a tracked vehicle and both sprockets are connected to DC motors and each one has a 1024 pulses relative encoder. The sprocket diameter is 140 mm and each full revolution requires 4000 pulses. The gear ratio is 30. Both encoders have an integer counter that increase or decrease depending on the sprockets direction.

           pulse_rev_left = 4000;
           pulse_rev_right = 4000;
           sprocket = 0.14;
        //Convert encoder string in long long integer
    std::string vel_enco=enco;
    std::replace(vel_enco.begin(), vel_enco.end(), ':', ' ');

    const char * c_enco = vel_enco.c_str();
    char* pEnd;
    double Encoder_sx, Encoder_dx;
    double Distance_sx, Distance_dx, Distance_prev_sx, Distance_prev_dx, Vel_sx, Vel_dx;
    float Vel_linear, w;

    Encoder_sx = std::strtod (c_enco,&pEnd);
    Encoder_dx = std::strtod (pEnd,&pEnd);
    //printf("E1 %lf, E2 %lf", Encoder_sx, Encoder_dx);

    Encoder_sx=  Encoder_sx/pulse_rev_left;
    Encoder_dx=  Encoder_dx/pulse_rev_right;
    Distance_sx = (Encoder_sx)*sprocket;
    Distance_dx = (Encoder_dx)*sprocket;

    last_time =ros::Time::now().toSec();
    Vel_sx = ((Distance_prev_sx - Distance_sx) / (last_time - current_time))/1000; // m / sec 
    Vel_dx = ((Distance_prev_dx - Distance_dx) / (last_time - current_time))/1000;  

    Distance_prev_sx = Distance_sx;
    Distance_prev_dx = Distance_dx;

    Vel_linear = (Vel_dx + Vel_sx)/2;
    w = (Vel_dx - Vel_sx)/track;

    ros::Time time2 = ros::Time::now();

    //Publish velocity topic
    ssvel << time2 << "," << Distance_sx << "," << Distance_dx << "," << Vel_sx << "," << Vel_dx << "," << Vel_linear << "," << w;
    msg_vel.data = ssvel.str();
    vel_pub.publish(msg_vel);

I had to remove the gear ratio value since the values I was getting were too high compared to the vehicle velocity which is about 0.8 m/s.

What is wrong with my code? How can I calculate x, y, vx and vy needed to feed the odometry topic?

EDIT:

I added the code to publish the odometry by using the ROS tutorial. However, I still haven't understand how I can improve and correct my code to use encoders in order to generate the odometry. Moreover, the tutorial uses th and vth while I only have vth, I guess.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

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

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

  pulse_rev_left = 4000;
  pulse_rev_right = 4000;
  sprocket = 0.14;

  double Distance_sx, Distance_dx, Distance_prev_sx, Distance_prev_dx, Vel_sx, Vel_dx;
  float Vel_linear, w;


  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();

  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();
        //Convert encoder string in long long integer
    std::string vel_enco=enco;
    std::replace(vel_enco.begin(), vel_enco.end(), ':', ' ');

    const char * c_enco = vel_enco.c_str();
    char* pEnd;
    double Encoder_sx, Encoder_dx;


    Encoder_sx = std::strtod (c_enco,&pEnd);
    Encoder_dx = std::strtod (pEnd,&pEnd);
    //printf("E1 %lf, E2 %lf", Encoder_sx, Encoder_dx);

    Encoder_sx=  Encoder_sx/pulse_rev_left;
    Encoder_dx=  Encoder_dx/pulse_rev_right;
    Distance_sx = (Encoder_sx)*sprocket;
    Distance_dx = (Encoder_dx)*sprocket;

    last_time =ros::Time::now().toSec();
    Vel_sx = ((Distance_prev_sx - Distance_sx) / (last_time - current_time))/1000; // m / sec 
    Vel_dx = ((Distance_prev_dx - Distance_dx) / (last_time - current_time ...