Skip to content

Instantly share code, notes, and snippets.

@JasonRBowling
Created September 24, 2022 16:30
Show Gist options
  • Save JasonRBowling/7d882d8f574ef3a30eb6be953273fdfd to your computer and use it in GitHub Desktop.
Save JasonRBowling/7d882d8f574ef3a30eb6be953273fdfd to your computer and use it in GitHub Desktop.
odom transform calcs
# compute the overall motion of the robot since last update
vec_v = (r_vel + l_vel) / 2.0
ang_vel = (r_vel - l_vel) / wheel_base
# decompose into x, y components of velocity vector
vx = vec_v * math.cos(ang_vel)
# calculate angular velocity
vy = vec_v * math.sin(ang_vel)
currentTime = rospy.get_rostime()
#print("Theta: " + str(theta) + " rad\t" + str(math.degrees(theta)) + " deg" )
# generate quaternion from yaw. Roll and pitch assumed to be zero.
odom_quat = tf.transformations.quaternion_from_euler(0, 0, theta)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform(
(x_position, y_position, 0.), odom_quat, currentTime, "base_footprint", "odom")
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = currentTime
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(
Point(x_position, y_position, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, ang_vel))
#set the covariance. The diagonal cannot be zero.
#used values and modified code from https://automaticaddison.com/how-to-publish-wheel-odometry-information-over-ros/
#and https://answers.ros.org/question/64759/covariance-matrix-for-vo-and-odom/
for i in range(0, 36):
odom.pose.covariance[i] = 0.0
odom.pose.covariance[0] = 0.01
odom.pose.covariance[7] = 0.01
odom.pose.covariance[14] = 0.01
odom.pose.covariance[21] = 0.1
odom.pose.covariance[28] = 0.1
odom.pose.covariance[35] = 0.1
# publish the message
odom_pub.publish(odom)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment