Greatly speeds up odometry and sonar inputs by fixing threading
Merge branch 'master' of github.com:rss-2014-team-6/rss-team-6