RP-248 Migrate Husky base to ros_control

Migrate to ros_control using horizon C++ API

Fix #5 #3 
Tradeoff here, keeping control and diagnostics (ros comms) on the same thread means we don't need to lock around hardware access.