RP-248 Migrate Husky base to ros_control

Migrate to ros_control using horizon C++ API

Fix #5 #3 
Consider publishing diagnostics instead?