wheel odometry publisher not rotating vehicle, some error in code. The forward and backward is working great only rotation is the problem.

asked 2022-04-04 07:22:50 -0500

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")
edit retag flag offensive close merge delete