# 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():

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

# next, we'll publish the odometry message over ROS

# 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!

edit retag reopen merge delete