Ask Your Question
1

How to compute odometry from encoder ticks

asked 2019-06-20 10:43:02 -0500

Shanika gravatar image

updated 2019-06-26 07:37:09 -0500

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
wheelradius = 0.0325
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)
odom_broadcaster = tf.TransformBroadcaster()

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:
        radius=dc/dth

        iccX=x-radius*sin(th)
        iccY=y+radius*cos(th)

        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
    odom_broadcaster.sendTransform(
       (x, y, 0.),
       odom_quat,
       current_time,
       "base_link",
       "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

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

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

    odom.child_frame_id = "base_link"
    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()

Thank you in advance,

Shanika

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-06-21 16:53:36 -0500

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-06-20 10:43:02 -0500

Seen: 114 times

Last updated: Jun 26