Rosanswers logo

I have tried all the solutions but can't seem to make it work, I can see the odometry in rviz with respect to the /odom frame, and see the tf go around in circles but it doesn't move with respect to the base_link frame. When I try to plot it in MATLAB, I get this plot, this is the robot going straight.

Complete code:Odometry.cpp alt text

alt text

    void update_velocities(
                       double dt)
    if (dt > 0) {
        int curr_encoder_count_left =
        int curr_encoder_count_right =
        double cos_current = cos(theta);
        double sin_current = sin(theta);

        // relying on the compiler for wraparound here
        int dist_left_counts =
            curr_encoder_count_left -
        int dist_right_counts =
            curr_encoder_count_right -

        if (dist_left_counts != dist_right_counts) {
            // convert from counts to standard units
            double dist_left_mm =
                (double)dist_left_counts /
            double dist_right_mm =
                (double)dist_right_counts /

            double right_left = dist_right_mm - dist_left_mm;

            double a =
                wheelbase_mm * (dist_right_mm + dist_left_mm) *
                0.5 / right_left;

            double fraction = right_left / wheelbase_mm;
            const double mm_to_m = 1.0 / 1000;
            vx = a * (sin(fraction + theta) - sin_current) *
            vy = -a * (cos(fraction + theta) - cos_current) *
            vtheta = fraction;

            // ROS_INFO("pose %.3f %.3f orientation %.3f",
            //  (float)x, (float)y, (float)theta);
            ROS_DEBUG("Encoders %.3f %.3f",
                      dist_left_mm, dist_right_mm);
        else {

        // store previous values
        previous_encoder_count_left = curr_encoder_count_left;
        previous_encoder_count_right = curr_encoder_count_right;

     ros::init(argc, argv, "phidgets_odometry");

        ros::NodeHandle n;
        ros::NodeHandle nh("~");

        std::string name = "odom";
        nh.getParam("name", name);

        std::string reset_topic = "odometry/reset";
        nh.getParam("reset_topic", reset_topic);

        nh.getParam("rotation", rotation_offset);

        n.setParam(reset_topic, false);

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

        // get encoder indexes
        encoder_index_left = -1;
        nh.getParam("encoderindexleft", encoder_index_left);
        encoder_index_right = -1;
        nh.getParam("encoderindexright", encoder_index_right);

        int direction = -1;
        nh.getParam("encoderdirectionleft", direction);
        if (direction!=-1) encoder_direction_left = 1;
        if (direction!=1) encoder_direction_right = -1;

        // connect to the encoders
        bool subscribed_to_encoders = false;
        if ((encoder_index_left > 0) &&
            (encoder_index_right > 0) &&
            (encoder_index_left!=encoder_index_right)) {
            subscribed_to_encoders =
        else {
            subscribed_to_encoders = subscribe_to_encoders();

        if (subscribed_to_encoders) {

            std::string base_link = "base_link";
            nh.getParam("base_link", base_link);
            std::string frame_id = "odom";
            nh.getParam("frame_id", frame_id);
            left_encoder_counts_per_mm = 0;
            right_encoder_counts_per_mm = 0;
            if (left_encoder_counts_per_mm < 1) {
                left_encoder_counts_per_mm = 1000;
            if (right_encoder_counts_per_mm < 1) {
                right_encoder_counts_per_mm = 1000;
            wheelbase_mm = 0;
            nh.getParam("wheelbase", wheelbase_mm);
            if (wheelbase_mm < 1) wheelbase_mm = 400;

            nh.getParam("verbose", verbose);
            nh.setParam("verbose", false);

            int frequency = 25;
            nh.getParam("frequency", frequency);

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

            initialised = true;

            ros::Rate update_rate(frequency);
                // reset the pose
                bool reset = false;
                n.getParam(reset_topic, reset);
                if (reset) {
                    x = 0;
                    y = 0;
                    theta = 0;
                    vx = 0;
                    vy = 0;
                    vtheta = 0;
                    n.setParam(reset_topic, false);
                    start_encoder_count_left = 0;
                    start_encoder_count_right = 0;

                current_time = ros::Time::now();
                double dt = (current_time - last_time).toSec();
                // update the velocity estimate based upon
                // encoder values

                // compute odometry in a typical way given
                // the velocities of the robot       
                x += vx;
                y += vy;
                theta += vtheta;
                // since all odometry is 6DOF we'll need a
                // quaternion created from yaw
                geometry_msgs::Quaternion odom_quat =
                // first, we'll publish the transform over tf
                geometry_msgs::TransformStamped odom_trans;
                odom_trans.header.stamp = current_time;
                odom_trans.header.frame_id = frame_id;
                odom_trans.child_frame_id = base_link;
                odom_trans.transform.translation.x = x;
                odom_trans.transform.translation.y = y;
                odom_trans.transform.translation.z = 0.0;
                odom_trans.transform.rotation = odom_quat;
                // send the transform
                // next, we'll publish the odometry
                //  message over ROS
                nav_msgs::Odometry odom;
                odom.header.stamp = current_time;
                odom.header.frame_id = frame_id;
                // set the position
                odom.pose.pose.position.x = x;
                odom.pose.pose.position.y = y;
                odom.pose.pose.position.z = 0.0;
                odom.pose.pose.orientation = odom_quat;
                // set the velocity
                odom.child_frame_id = base_link;
                odom.twist.twist.linear.x = vx/dt;
                odom.twist.twist.linear.y = vy/dt;
                odom.twist.twist.angular.z = vtheta/dt;

                // publish the message

                last_time = current_time;

Originally posted by fiorano10 on ROS Answers with karma: 45 on 2017-09-20

Post score: 0


1 Answer 1


Rosanswers logo

I'm not sure if this is your problem but why do you only compute the odometry in update_velocities() when your encoder counts for each wheel are different? ie: if (dist_left_counts != dist_right_counts) { ... }

If you are driving in a straight line isn't it possible that your left and right wheels would have the same number of encoder counts?

EDIT 1: Upon looking at your code I believe your math might be wrong in your wheel_velocities(). Try this instead

   const double mm_to_m = 1.0 / 1000.0; 
   double wheelbase_m = wheelbase_mm * mm_to_m; //to avoid issues
   double right_left = (dist_right_mm - dist_left_mm) * mm_to_m; //get this is meters

    double a =  (dist_right_mm + dist_left_mm) * 0.5 * mm_to_m ; //get this is meters

    double fraction = right_left / wheelbase_m;

    //assuming theta is the previous yaw value
    vx = a * cos(theta + (fraction/2.0));
    vy = a * sin(theta + (fraction/2.0));
    vtheta = fraction;

This is the standard diff drive odom model. I believe you may have had issues from units and your cos and sin were backwards.

Originally posted by biglotusturtle with karma: 165 on 2017-09-21

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by fiorano10 on 2017-09-21:
It is possible so I tried running it without that condition and I'm still getting the same output, I can see the tf and odometry revolve in circles but they don't move in with respect to base link, could it be a transform issue ? I've added the tf tree

Comment by biglotusturtle on 2017-09-21:
May be a tf issue. Print your x,y, and theta values out to the command window and check that the values look reasonable. Also try printing out your wheel velocity values and do a sanity check on those. Maybe one of your encoders is bad?

Comment by fiorano10 on 2017-09-21:
Can you take a look at this Odometry_data

Comment by biglotusturtle on 2017-09-21:
Looks like your velocities are almost always 0.0. Try getting rid of that reset condition/param in your while loop? Also what is the output of your ROS_DEBUG("Encoders %.3f %.3f",dist_left_mm, dist_right_mm); ? Do your wheel distances seem reasonable?

Comment by fiorano10 on 2017-09-25:
I got rid of the reset condition, made the changes you suggested in EDIT 1, I'm getting this https://youtu.be/oeEnLN81uJU. Also, when I display messages in rqt_console, I can't see Debug messages ? How do I display the distance ?

Comment by biglotusturtle on 2017-09-26:
Change your print statement to ROS_INFO("Encoders %.3f %.3f", dist_left_mm, dist_right_mm);

And make sure output="screen" in your launch file

Based on the video it is looking like you encoder are returning bad data.

Comment by fiorano10 on 2017-09-26:
I did manage to get the dist_left/dist_right: https://drive.google.com/open?id=0B9o1xpG4iKlGUzhmWGJtY2Y1c1k Log files: https://drive.google.com/file/d/0B9o1xpG4iKlGWXJ4eEZSYWpqb0U/view?usp=sharing https://drive.google.com/file/d/0B9o1xpG4iKlGZ2MtaHd0VlZmdzQ/view?usp=sharing

Comment by biglotusturtle on 2017-09-26:
How often is the dist_left_mm and dist_right_mm being computed? I am seeing values of 10 mm? Which would be the same as 0.3 m/s at 30 hz? Does that seem reasonable for your wheel sizes? Also you wont necessarily see data in rqt-reconfigure. Ros_info should show up in your command window.

Comment by fiorano10 on 2017-09-27:
The wheels are 3 inches~76.2mm in radius and the frequency is 25Hz, so the distances are being calculated at an update rate of the same. How do I see the pose with respect to the map frame ?

Comment by biglotusturtle on 2017-09-27:
not sure what your frame_id is set to in odom_trans but assuming that it is set to "odom" you can set your fixed frame to "odom" in rviz and then you should see your base_link frame move relative to it.

Comment by fiorano10 on 2017-09-27:
I wasn't able to see that earlier, now I can ! Thank you.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.