Not able to calculate orientation properly (odom) [closed]

edit

I am new to robotics and ROS and am facing problem to calculate the pose of my robot. It is similar to turtlebot.

I am able to get the linear distance properly, but it is incorrect in terms of rotation. I am using stepper motors, hence have easy access to steps, which i use as encoder value. So, for example, if number of steps is 44, and the radius of wheel is 3.5 cm, then the linear distance the robot base moved is 0.048380527 meters. This is what i get from my odom code. But, I am unable to get correct theta (yaw). So, for example if the right wheel moves -47 steps and left wheel moves 47 steps, then the base take a right turn with ICR lying at the centre of base and the angle in radians should be -0.22469217. But I get an output of -0.901602880782.

Following is my code for calculating odometry. What is wrong?

#!/usr/bin/env python
import math
from math import sin, cos, pi

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

tick_x = 0
tick_y = 0 
DistancePerCount = (pi * 0.07) / 200
lengthBetweenTwoWheels = 0.46
th = 0
x = 0
y = 0
_PreviousLeftEncoderCounts = 0
_PreviousRightEncoderCounts = 0

def posecalc1 (tick1):
    global tick_x
    tick_x = int(tick1.data)

def posecalc2 (tick2):
    global tick_y
    tick_y = int(tick2.data)


rospy.init_node('pubs_ard', anonymous=True)
sub1 = rospy.Subscriber("lwheel", Float32, posecalc1)
sub2 = rospy.Subscriber("rwheel", Float32, posecalc2)
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
r = rospy.Rate(10)
odom = Odometry()

while not rospy.is_shutdown():

    odom_broadcaster = tf.TransformBroadcaster()

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


    deltaLeft = tick_x - _PreviousLeftEncoderCounts
    deltaRight = tick_y - _PreviousRightEncoderCounts

    omega_left = (deltaLeft * DistancePerCount) / (current_time.secs - last_time.secs)
    omega_right = (deltaRight * DistancePerCount) / (current_time.secs - last_time.secs)

    v_left = omega_left #* 0.035 #radius
    v_right = omega_right# * 0.035

    vx = ((v_right + v_left) / 2)
    vy = 0
    vth = ((v_right - v_left)/lengthBetweenTwoWheels)*10

    #compute odometry in a typical way given the velocities of the robot

    dt = (current_time.secs - last_time.secs)
    delta_x = (vx * cos(th)) * dt
    delta_y = (vx * sin(th)) * dt
    delta_th = vth * dt

    x += delta_x
    y += delta_y
    th += delta_th

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    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_footprint",
        "odom"
    )

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

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

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

    odom_pub.publish(odom)

    _PreviousLeftEncoderCounts = tick_x
    _PreviousRightEncoderCounts = tick_y

    last_time = current_time
    r.sleep()

Thanks a lot!