I am using a turtlebot 3. I am controlling it with ROS Noetic running on a laptop running Ubuntu 20.04 with an intel i5-11th gen CPU. 

Hello!

I am trying to move my turtlebot3 exactly 40 cm forward. However, my robot is moving -/+ 3cm, which is not acceptable for my application.

As you can see from my script, I print the total distance moved (which I calculate from the odometry data) EXACTLY AFTER the robot stops. Then, I put a time.sleep for 2 seconds, and re-calculate the total distance moved and I print it again. As I understand, there should be no difference between these 2 calculations as the robot has not moved at all, however, they are not identical.

I guess this difference could be caused by a delay in the odometry thread? but to be honest I am a newbie to ROS and would greatly appreciate your help :)

Could anyone please tell me if there's any way to fix this or if I am making a mistake on the way I read my odometry data? Thank you!

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi, atan2
from tf.transformations import euler_from_quaternion
import numpy as np
import time


# Get odom information
def get_odom():
    try:
        (trans, rot) = tf_listener.lookupTransform(odom_frame, base_frame, rospy.Time(0))
        rotation = euler_from_quaternion(rot)

    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        rospy.loginfo("TF Exception")
        return

    return (Point(*trans), rotation[2])


# Move forward 40cm
def moveDiscrete():
    global move_cmd
    global cmd_vel

    (position, rotation) = get_odom()
    init_x = position.x
    init_y = position.y

    distance = 0.4  # meters ==> 40cm
    movedDist = 0

    # As long as I am moer than 5cm away, keep moving!
    while (distance - movedDist) > 0.05:   
        (position, _) = get_odom()   # Updat eodometry

        # Calculate euclidean distance
        movedDist = sqrt(pow(init_x - position.x, 2) + pow(init_y - position.y, 2))   

        move_cmd.linear.x = 0.15  # Set Speed
        cmd_vel.publish(move_cmd) # Move

        r.sleep()  # use rospy sleep function to keep while loop running at desired rate

    rospy.loginfo("Done")


# Main
rospy.init_node('ttb3_test', anonymous=False)
cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
position = Point()
move_cmd = Twist()
r = rospy.Rate(20)  # I tried setting to 2, 10, 20. Results are the same.
tf_listener = tf.TransformListener()
odom_frame = 'odom'


# While Loop
while not rospy.is_shutdown():
    try:
        tf_listener.waitForTransform(odom_frame, 'base_footprint', rospy.Time(), rospy.Duration(1.0))
        base_frame = 'base_footprint'
    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        try:
            tf_listener.waitForTransform(odom_frame, 'base_link', rospy.Time(), rospy.Duration(1.0))
            base_frame = 'base_link'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")
            rospy.signal_shutdown("tf Exception")

    # Odometry
    (position, rotation) = get_odom()
    init_x, init_y = position.x, position.y
    last_rotation = 0
    linear_speed = 1
    angular_speed = 1

    # Move 40 centimeters
    moveDiscrete()

    (final_position, rotation) = get_odom()
    print("Distance travelled ==> ", sqrt(pow(init_x - final_position.x, 2) + pow(init_y - final_position.y, 2)))

    time.sleep(2)

    (final_position, rotation) = get_odom()
    print("Distance travelled ==> ", sqrt(pow(init_x - final_position.x, 2) + pow(init_y - final_position.y, 2)))

    break