Hydro devel

Note that velocities from ROS are assumed to be in the body frame. In the future we should add an option for this.