Ask Your Question
0

How to calculate odometry using encoders ?

asked 2017-09-20 18:11:03 -0600

fiorano10 gravatar image

updated 2017-11-15 16:27:34 -0600

jayess gravatar image

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 =
            current_encoder_count_left;
        int curr_encoder_count_right =
            current_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 -
            previous_encoder_count_left;
        int dist_right_counts =
            curr_encoder_count_right -
            previous_encoder_count_right;

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

            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) *
                mm_to_m;
            vy = -a * (cos(fraction + theta) - cos_current) *
                mm_to_m;
            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 {
            vx=vy=vtheta=0;
        }

        // 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;
        direction=1;
        nh.getParam("encoderdirectionright",
                    encoder_direction_right);
        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 =
                subscribe_to_encoders_by_index();
        }
        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;
            nh.getParam("countspermmleft",
                        left_encoder_counts_per_mm);
            if (left_encoder_counts_per_mm < 1) {
                left_encoder_counts_per_mm = 1000;
            }
            nh.getParam("countspermmright",
                        right_encoder_counts_per_mm);
            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);
            while(ros::ok()){
                // 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
                update_velocities(dt);

                // 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 =
                    tf::createQuaternionMsgFromYaw(theta);

                // 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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-09-21 11:51:18 -0600

biglotusturtle gravatar image

updated 2017-09-21 15:19:22 -0600

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.

edit flag offensive delete link more

Comments

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

fiorano10 gravatar imagefiorano10 ( 2017-09-21 12:09:50 -0600 )edit

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?

biglotusturtle gravatar imagebiglotusturtle ( 2017-09-21 12:17:14 -0600 )edit

Can you take a look at this Odometry_data

fiorano10 gravatar imagefiorano10 ( 2017-09-21 14:25:16 -0600 )edit

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?

biglotusturtle gravatar imagebiglotusturtle ( 2017-09-21 14:37:12 -0600 )edit

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 ?

fiorano10 gravatar imagefiorano10 ( 2017-09-25 15:28:56 -0600 )edit

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.

biglotusturtle gravatar imagebiglotusturtle ( 2017-09-26 12:16:31 -0600 )edit
fiorano10 gravatar imagefiorano10 ( 2017-09-26 14:01:33 -0600 )edit

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.

biglotusturtle gravatar imagebiglotusturtle ( 2017-09-26 20:34:59 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2017-09-20 18:11:03 -0600

Seen: 2,110 times

Last updated: Nov 15 '17