wheel odometry publisher not rotating vehicle, some error in code. The forward and backward is working great only rotation is the problem.
It goes forward and backward when rotating the robot. I am using wheel rpm values to convert to odometry data please guide if theres anything wrong or i have missed out something.
please check the code i think i couldnt spot the error in the code.
import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
from std_msgs.msg import Float32
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
import tf
import math
def leftVelCallback(vel):
global leftVel
leftVel = vel.data
def rightVelCallback(vel):
global rightVel
rightVel = vel.data
rospy.init_node('odometry_publisher')
odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
rospy.Subscriber("/left_vel", Float32, leftVelCallback)
rospy.Subscriber("/right_vel", Float32, rightVelCallback)
odom_broadcaster = tf.TransformBroadcaster()
x = 0.0
y = 0.0
th = 0.0
vx = 0
vy = 0
vth = 0
leftVel = 0
rightVel = 0
wheeltrack = 0.63
wheelradius = 0.10
current_time = rospy.Time.now()
last_time = rospy.Time.now()
r = rospy.Rate(5)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
average_rpm_x = (leftVel + rightVel) / 2
average_rps_x = average_rpm_x / 60
linear_velocity = (average_rps_x * (2 * math.pi * wheelradius))
average_rpm_a = (rightVel - leftVel)
average_rps_a = average_rpm_a / 60
angular_velocity = (average_rps_a * ( 2 * math.pi * wheelradius)) / wheeltrack
vx = linear_velocity
vy = 0
vth = angular_velocity
print(vx)
print(vth)
# //compute odometry in a typical way given the velocities of the robot
dt = (current_time - last_time).to_sec()
delta_x = (vx * math.cos(th) - vy * math.sin(th)) * dt
delta_y = (vx * math.sin(th) + vy * math.cos(th)) * dt
delta_th = vth * dt
x += delta_x
y += delta_y
th += delta_th
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# # //send the transform
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))
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
# //publish the message
odom_pub.publish(odom)
last_time = current_time
r.sleep()
rospy.loginfo("Published Odom")