odom transform calcs

gistfile1.txt

# 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)