Error in Wheel encoder Odometry

edit

Hello,

I m working on a Differential drive robot for navigation. For which, I m publishing the encoder ticks from Arduino Mega over rosserial and computing the /Odom using a python node on RPi4. To check whether my odometry is working properly, I tested my robot with the given instruction on the below link: navigation troubleshhoting

There seems to be a problem with my Odom as the laser scans are not overlapping each other. Here is my Odom publisher script:

    #!/usr/bin/env python3
    # -*- coding: utf-8 -*-
    import math
    from math import sin, cos, pi

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

    rospy.init_node('odometry_publisher')

    l = 0.385
    rc = 0.68
    lc = 0.68
    ppr = 27
    dl = 0.0
    dr = 0.0
    dt = 0.0
    th = 0.0
    x  = 0.0
    y  = 0.0
    dth = 0.0
    dc = 0.0
    dx = 0.0
    dy = 0.0
    vx = 0.0
    vy = 0.0
    vth = 0.0
    dlticks = 0
    drticks =0
    lticks = 0
    rticks = 0

    last_lticks = 0
    last_rticks = 0

    def l_ticks_Callback(msg):
        global lticks
        lticks = msg.data  
        # dl = (msg.data/ppr)*lc*(0.15/0.23)   

    def r_ticks_Callback(msg):
        global rticks
        rticks = msg.data
        # dr = (msg.data/ppr)*rc*(0.15/0.23)

    def timeCallback(msg):
        global dt 
        dt = msg.data/1000

    odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
    dl_pub = rospy.Publisher("dl", Float64, queue_size=50)
    dr_pub = rospy.Publisher("dr", Float64, queue_size=50)
    dl_sub = rospy.Subscriber("/l_ticks", Int64, l_ticks_Callback) 
    dr_sub = rospy.Subscriber("/r_ticks", Int64, r_ticks_Callback)
    dt_sub = rospy.Subscriber("/time_stamp", Float64, timeCallback)

    odom_broadcaster = tf.TransformBroadcaster()

    r = rospy.Rate(50)

    while not rospy.is_shutdown():
        current_time = rospy.Time.now()
        dlticks = (lticks - last_lticks) 
        drticks = (rticks - last_rticks) 
        dl = (dlticks/ppr)*(lc)
        dr = (drticks/ppr)*(rc)

        dc = (dl + dr) / 2

        dth = (dr-dl)/l

        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)

        odom_broadcaster.sendTransform(
           (x, y, 0.),
           odom_quat,
           current_time,
           "base_footprint",
           "odom"
        )

        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>l:
           vx=dx/dt
           vy=dy/dt
           vth=dth/dt

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

        odom_pub.publish(odom)
        dl_pub.publish(dlticks)
        dr_pub.publish(drticks)
        last_lticks = lticks
        last_rticks = rticks
        r.sleep()

Are the calculations correct? Please help me to solve this error.