Publishing odometry for 6w_Drive plugin

Uncommenting publish_odometry() function. Not sure if this was originally done on purpose or not.