The kinematic equations can be found for example by searching for differential drive robots.
E.g. one of the first results: diff-drive kinematics.
Your velocities at the left and right wheel are denoted as v_l and v_r respectively.
Velocities in the robot frame (which are located in the center of rotation, e.g. base_link frame)
v_rx = ( v_right + v_left ) /2;
v_ry = 0; // we have a non-holonomic constraint (for a holonomic robot, this is non-zero)
omega_r = ( v_right - v_left ) / d; // d denotes the distance between both wheels (track)
Subscript r indicates the robot frame.
Note, after converting your encoder values to rotational velocities at each wheel (omega_left, omega_right),
you can transform them to the translational velocity at each wheel using the radius of the wheel: v_left = omega_left * radius.
Velocities in the odom frame
The velocities expressed in the robot base frame can be transformed into another coordinate system (e.g. odom).
For transforming velocities (in this simple setting with scalar values), we only need to rotate them according
to the current robot orientation. Here we apply the 2D rotation matrix (we are operating in the x-y plane).
We assume that we already know the current orientation (theta).
v_wx = v_rx * cos(theta) - v_ry * sin(theta);
v_wy = v_rx * sin(theta) + v_ry * cos(theta);
thetadot = omega_r;
For your robot kinematics it is v_ry=0. Subscript w indicates the world/odom frame.
Computing the current robot pose
In order to obtain the current robot position, numerical integration can be applied.
The velocities are integrated in order to achieve the current pose (x, y, theta).
We define the pose in the odom frame, and start with x=y=theta=0 (origin of the odom frame).
Applying Explicit/Forward Euler Integration
leads to
x_kp1 = x_k + v_wx * dt; // kp1 := k+1
y_kp1 = y_k + v_wy * dt; // by the way, the last term is delta_y from the equations in your comment
theta_kp1 = theta_k + thetadot * dt;
In your implementation, you can simply share the same variables for x_kp1 and x_k etc.
dt denotes the step width and should be choosen according to the time elapsed since the last integration step (here you might see, that your rate significantly influences your integration accuracy. But the integration rate is limited by the rate of your sensors (wheel encoders).
Composing your odometry message
The odometry message contains the pose and the twist information.
Now you should have everything to compute the values.
The pose represents your current robot pose w.r.t the odom frame (x_k, y_k, theta_k). You must convert theta_k to a Quaternion, e.g. by calling the related functions of the tf package.
The twist part usually contains the velocities expressed in your robot frame (there is an extra parameter where you can insert the frame name (child_frame_id), but not all drivers are filling it with a value.
Here you just need the linear velocity (linear.x = v_rx and the angular velocity (around the z-axis, angular.z = omega_r).
All other values are zero.
You can find much information on those things!
(more)
