# Publish odometry data based on encoders

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

I'm missing the wheel size in your description and the code. Also last_time looks strange to me, but I have not fully parsed your code yet. I would not even bother with calculation of wheel size, pulses per revolution or gear ratio. Instead I would just drive 1m and divide that by the number of encoder pulses for each motor because when you say your wheel size is 3cm it will not be exactly 3cm, but e.g. 3,0001cm or 2.999cm. Also I recommend to copy the code from the tutorial

Thank you for your answer and your support! I do not have wheels since it's a tracked vehicle, the sprocket (drive wheel) of each track has a diameter of 140 mm. I already checked that tutorial but it doesn't explain how to handle the encoder ticks to calculate the velocity and the distance.

There is no real difference between wheels and tracks. Just the slip when rotating, I hope you have an IMU to correct this. Unless your sprocket touches so ground you need the diameter of the sprocket including the track.

Yes, I have a imu and also a GPS, but at the moment, I only need to generate odometry data from encoders. I would like to understand how to use the ticks to calculate the distance and the velocity since my code isn't working very well. I will use imu and gps later to correct the encoders data.

Did you thought about just driving 1/5/10m and counting the ticks? Also please copy the code from the tutorial linked above.

Yes, I cover 100 cm with 9302 pulses. I edited my first post by adding the code from tutorial. I can't understand how to calculate

`th`

and`vth`

. In my code I can only calculate`vth`

as difference between the left and right velocity but I can't get the angle, too.This is basic math I tried to explain here. But there are probably better texts about that, e.g. this Wheels or chains is no difference here.

Just occurred to me: Are you reading the encoders on a Linux PC? Are you sure that you can read all encoder ticks without any data loss? This is usually done in a real time environment like a separate microcontroller.