# Generating odom message from encoder ticks for robot_pose_ekf

Hi all,

I am using robot_pose_ekf to get the estimate of the robot pose and is slightly confused on how to use encoder ticks to generate the odometry message. Till now, I am able to achieve the following:

1. The encoder ticks are being published on a certain topic ("re_ticks").
2. I am able to find the velocity of the left wheel and the right wheel (vel_left, vel_right) using wheel properties and encoder ticks.
3. Using 2, I found out Rotational and Translation velocity of the robot (V,W). (not sure about its correctness)

Now, I have to find velocity of the robot in X,Y and theta direction so that I can use http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom to generate the odom message.
vtheta = Wt but I am not sure about vx and vy. Intuitively, it seems that I should just do vx = Vcos(theta) and vy = Vsin(theta) but I am not sure. Also, which theta should I use, the old one or the current theta (theta = Wdt)?

I have attached the relevant part of the code below:

void rotary_encoders_callback(const geometry_msgs::Vector3::ConstPtr& ticks)
{

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

double delta_left = ticks->x - previous_left_ticks;
double delta_right = ticks->y - previous_right_ticks;

// dist_per_count = distance traveled per count, delta_left = ticks moved
double vel_left = (delta_left * dist_per_count) / (current_time_encoder - last_time_encoder).toSec(); // Left velocity
double vel_right = (delta_right * dist_per_count) / (current_time_encoder - last_time_encoder).toSec(); // Right velocity

// Getting Translational and Rotational velocities from Left and Right wheel velocities
// V = Translation vel. W = Rotational vel.
if (vel_left == vel_right)
{
V = vel_left;
W = 0;
}

else
{
// Assuming the robot is rotating about point A
// W = vel_left/r = vel_right/(r + d), see the image below for r and d
double r = (vel_left * d) / (vel_right - vel_left); // Anti Clockwise is positive
W = vel_left/r; // Rotational velocity of the robot
V = W * (r + d/2); // Translation velocity of the robot
}

vth = W;
// Find out velocity in x,y direction (vx,vy)
// ???

previous_left_ticks = ticks->x;
previous_right_ticks = ticks->y;

last_time_encoder = current_time_encoder;
}


AB = r, BC = d

So, I want to make sure if everything till now is correct and if it is correct, how can I find velocity in x and y direction (vx,vy) so that I can use that to generate the odom message?

Naman Kumar

Originally posted by Naman on ROS Answers with karma: 1464 on 2015-04-19

Post score: 2

Comment by Mehdi. on 2016-03-15:
How do you estimate r from the odom Twist? I need to calculate the number of ticks from the odom message so basically the opposite of what you want to do.

Your calculation are correct but you should pay attention to update frequency: if it is too high respect to wheel velocities your calculated speeds will be unstable. Just and example: your robot is moving at 0.01 m/s. If you have 0.1 radius wheels and 360 ticks per round encoders ticks increase by 11 every second. If you update velocity at 20Hz means that sometime deltas will be zero, some time one, where your robot is moving at constant speed. Same happens when the thick from encoders are not synchronised.

To avoid this you should add an update system that wait for consistent data available.

For your information I did the same in different way. I'm using MD25 board that include close loop control of motors so I can be sure my speed input results in fixed speed. I use the input to MD25 to calculate speeds instead to calculate by real thicks.

## UPDATE

Another way is to publish encoder ticks only if there is a change in the encoder position then deltas are never zero, right?

No, you will have same problem! The best you can do un order to be sure about results is to run your code and drive your robot at minimal speed. If detected speeds are constant all is fine. If output is not stable... you have to think about possible fixes.

Originally posted by afranceson with karma: 497 on 2015-04-20

This answer was ACCEPTED on the original site

Post score: 1