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, 1) vy = round(message.velocity, 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.