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

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!

it will probably help forum members (not necessarily me) if you spend a few words describing what you think is wrong, what you tried, why that didn't work, etc.

Thanks for the feedback @gvdhoorn. I have added more details to explain the situation

Hi @parzival I am having same trouble. Could you please let me know how did you solve this problem.