# How to compute odometry from encoder ticks

Hello,

I would like to know how to publish odom by subscribing to my robot's right and left encoder ticks for a differential drive and visualize in rviz.

I saw this topic Odom Publisher by Subscribing Encoder Ticks for Differential Drive but nobody answered to the post's author. Are the calculations correct ?

Arduino : Encoder Ticks Publishers
The code can be found on my previous question post

Python : Publish odom

import math
from math import sin, cos, pi

import rospy
import tf
from nav_msgs.msg import Odometry
from std_msgs.msg import Int16
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3

#Parameters
wheeltrack = 0.143
TPR = 351
left_ticks = 0
right_ticks = 0
last_left_ticks = 0
last_right_ticks = 0

x = 0.0
y = 0.0
th = 0.0

vx =  0.0
vy =  0.0
vth =  0.0

def leftTicksCallback(msg):
global left_ticks
left_ticks = msg.data

def rightTicksCallback(msg):
global right_ticks
right_ticks = msg.data

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
left_ticks_sub =rospy.Subscriber("/left_ticks", Int16, leftTicksCallback)
right_ticks_sub =rospy.Subscriber("/right_ticks", Int16, rightTicksCallback)

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(10)

while not rospy.is_shutdown():
current_time = rospy.Time.now()

delta_L = left_ticks - last_left_ticks
delta_R = right_ticks - last_right_ticks
dl = 2 * pi * wheelradius * delta_L / TPR
dr = 2 * pi * wheelradius * delta_R / TPR
dc = (dl + dr) / 2
dt = (current_time - last_time).to_sec()
dth = (dr-dl)/wheeltrack

if dr==dl:
dx=dr*cos(th)
dy=dr*sin(th)

else:

dx = cos(dth) * (x-iccX) - sin(dth) * (y-iccY) + iccX - x
dy = sin(dth) * (x-iccX) + cos(dt) * (y-iccY) + iccY - y

x += dx
y += dy
th =(th+dth) %  (2 * pi)

odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

# first, we'll publish the transform over tf
(x, y, 0.),
odom_quat,
current_time,
"odom"
)

# next, we'll publish the odometry message over ROS
odom = Odometry()

odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

if dt>0:
vx=dx/dt
vy=dy/dt
vth=dth/dt

odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

odom_pub.publish(odom)

last_left_ticks = left_ticks
last_left_ticks = right_ticks
last_time = current_time
r.sleep()

Shanika

edit retag close merge delete

Did you solve the problem? If do, could you please share the code?

Sort by » oldest newest most voted If you are planning to do it using only encoders. Then you can refer to it. You can use gazebo for getting odom by supplying real world data to gazebo through controllers in ros. https://youtu.be/ihuSkFIn7YU

more

Hi Shanika,

I tested this code, it works but you have a problem at the end: A copy-paste error, last_left_ticks = right_ticks, should be last_right_ticks = right_ticks

more
more