Generating odom message from encoder ticks for robot_pose_ekf

edit

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/Tutori... 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?

Thanks in advance.
Naman Kumar