Odometry drift and how to fix

edit

Asking for advice on how should I fix the odometry drifting of the wheel. I detail of my problems are described below.

I have a differential-drive robot using RaspberryPi-4 and Oriental motor and motor-driver. The motor driver and the RaspberryPi are connect using RS-485 communication and python minimalmodbus library is used to drive and read speed of the motor.

The Kinematics model using is:

# inverse kinematics
velocity_left_wheel = (linear_velocity - angular_velocity*wheel_distance/2)/wheel_radius
velocity_right_wheel = (linear_velocity + angular_velocity*wheel_distance/2)/wheel_radius

# forward kinematics
robot_linear_velocity = (wheel_radius*(velocity_right_wheel + velocity_left_wheel)/2 )
robot_angular_velocity = (wheel_radius*(velocity_right_wheel - velocity_left_wheel)/wheel_distance )

In this way, I can drive and read the speed properly. When calculating the odometry, the robot location in real-world and RVIZ did not match(mostly during rotation). The odometry equation using is:

    delta_x = (robot_linear_velocity*np.cos(robot_th)) * dt
    delta_y = (robot_linear_velocity*np.sin(robot_th)) * dt
    delta_th = robot_angular_velocity * dt

    robot_x += delta_x
    robot_y += delta_y
    robot_th += delta_th

To read and write the speed of the motor, multi-callback functions are used:

    rospy.Timer(rospy.Duration(0.1), self.read_callback)
    rospy.Timer(rospy.Duration(0.1), self.write_callback)
    rospy.spin()

Is there any suggestion to fix my problems. I already upload the complete code in github. If you are interested, feel free to check and advise me. Thank you to all contributors.

Tried ways:

• using single callback and publish robot_velocity to Odometry node to calculate odometry and publish tf
• using single callback and odometry is calculated in the same node
• using multiple callback and odometry is calculated in the same node (already linked to github)