Rosanswers logo


I have an IMU which shows the orientation perfectly and also installed some optical encoders. When I move the robot straight, the encoders will show the movement more or less correctly. However, when I turn and then move, IMU first displays orientation then when I move after the turn suddenly my robot gets drifted back and then move. I guess that this is because encoders do their job separately but how can I fuse them together?

This is my encoder tick callback (one of them, second will be similar):

void Odometry::leftencoderCb(const std_msgs::Int64::ConstPtr& left_ticks)
    double enc = left_ticks->data;

    if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap))
        lmult = lmult + 1;

    if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap))
        lmult = lmult + 1;

    left = 1.0 * (enc + lmult * (encoder_max - encoder_min));
    prev_lencoder = enc;

Here is my IMU callback:

void Odometry::ImuCb(const sensor_msgs::Imu::ConstPtr& data)
    orient_x = data->orientation.x;
    orient_t = data->orientation.y;
    orient_z = data->orientation.z;
    orient_w = data->orientation.w;


And finally, here is my update code which updates odometry periodically:

void Odometry::update()

    ros::Time now = ros::Time::now();
    double elapsed;
    double d_left, d_right, d, th,x,y;

    elapsed = now.toSec() - then.toSec();

    if(enc_left == 0)
        d_left = 0;
        d_right = 0;
            d_left = (left - enc_left) / ( ticks_meter);
            d_right = (right - enc_right) / ( ticks_meter);

        enc_left = left;
        enc_right = right;

        d = (d_left + d_right ) / 2.0;
        th = ( d_right - d_left ) / base_width;

        dx = d / elapsed;
        dr = th / elapsed;

        if (d != 0)
            ROS_INFO("Distance has been changed!");
            x = cos(th) * d;
            y = -sin(th) * d;
            x_final += (cos(theta_final) * x - sin(theta_final) * y);
            y_final += (sin(theta_final) * x + cos(theta_final) * y);

        theta_final += th;

        geometry_msgs::Quaternion odom_quat ;

        odom_quat.x = this->orient_x;
        odom_quat.y = this->orient_y;
        odom_quat.z = this->orient_z;
        odom_quat.w = this->orient_w;

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = now;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        odom_trans.transform.translation.x = x_final;
        odom_trans.transform.translation.y = y_final;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;


            // Odometry values:
        nav_msgs::Odometry odom;
        odom.header.stamp = now;
        odom.header.frame_id = "odom";

        odom.pose.pose.position.x = x_final;
        odom.pose.pose.position.y = y_final;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        odom.child_frame_id = "base_footprint";
        odom.twist.twist.linear.x = dx;
        odom.twist.twist.linear.y = 0.0; //dy;
        odom.twist.twist.angular.z = 0.0; //dr;


        then = now;


    else ROS_INFO_STREAM("Not in a loop");

Can someone suggest me how can I write odometry code more correctly?

Originally posted by stevemartin on ROS Answers with karma: 361 on 2019-01-09

Post score: 0


1 Answer 1


Rosanswers logo

The short answer to this is to not write your own odometry fusion algorithm from scratch, unless you have to do this as part of an assignment or for research. The existing robot localization package is very powerful and is designed for doing exactly this.

You will need to modify your code so that your encoder information is published in a twist message so that it can be sent to the robot localization node, then you can follow the guide on their website to configure the sensor fusion. There is a bit of a learning curve when it comes to using this node, but it's worth the effort when it's setup properly.

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-01-09

This answer was ACCEPTED on the original site

Post score: 4

Original comments

Comment by gvdhoorn on 2019-01-09:\

You will need to modify your code so that your encoder information is published in a twist message

shouldn't that be nav_msgs/Odometry for a mobile base?

Comment by stevemartin on 2019-01-09:
Okay I get it, so basically I need to remove the IMU callback and let IMU be a separate thing from odometry, am I right? As without IMU, the orientation is terrible in Odometry.

Comment by stevemartin on 2019-01-09:
And then use odometry (which estimates the position based on encoder ticks) and IMU as two separate things and fuse them both to robot_localization package to get the filtered odometry?

Comment by PeteBlackerThe3rd on 2019-01-09:
@gvdhoorn yes that's correct. A twist message would not be able to represent the absolute orientation.

Comment by PeteBlackerThe3rd on 2019-01-09:
@stevemartin yes the robot localization package will take in the odometry and imu messages and fuse them for you into a new odometry estimate. When correctly setup it will use an EKF to take the best of each sensor.


Your Answer

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