Firmware - Publish encoder feedback as Odom message

[haashimrehan]

To help with localization, I’d like to use the linear information from the Encoders. to minimize errors due to slippage, we can go about this by:

• Determining the slowest velocity motor on the left side
• Determining the slowest velocity motor on the right side
• with a left_vel and right_vel (m/s) we can calculate the linear portion of the twist message (Twist.linear.x)
• publish to a topic called /wheel_odom

Instead of taking the slowest velocity motor we could also take the average velocity per side.

Not interested in angular information in the wheel encoders since it’s a friction steer. We will use an IMU for that.

[emcauliffe]

Instead of a Twist message, encoder data is a nav_msgs/Odometry message. This provides Twist and Pose feedback with covariance matrices.