Hey,
i am trying to calculate odometry using wheel velocity published using the joint_state topic and compare the calculated position and rotation with the tf topic.
the problem i am having is that the calculated odometer quite large in scale when comparing to tf from isaac sim.
code I’m using for calculating odometry from wheel velocity.
dt = (current_time - last_time).to_sec()
vx = round(message.velocity[0], 1)
vy = round(message.velocity[1], 1)
v_rx = (vx + vy) / 2.0
v_ry = 0
omega_r = ( vx - vy ) / 4.5 # TODO proper distance between tires
# compute odometry in a typical way given the velocities of the robot
delta_x = (v_rx * cos(th) - v_ry * sin(th)) * dt
delta_y = (v_rx * sin(th) + v_ry * cos(th)) * dt
delta_th = omega_r * dt
# updating x, y and theta
x += delta_x
y += delta_y
th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(th / 2.0)
quaternion.w = cos(th / 2.0)
# Create the odometry transform frame broadcaster.
odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_footprint",
"world"
)
odom = Odometry()
odom.header.frame_id = "world"
odom.child_frame_id = "base_footprint"
odom.header.stamp = current_time
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
odomPub.publish(odom)
really appreciate any help.
Regards