Lots of bug fixes and clean ups.

I wasn't actually able to get everything building from source before these changes. I think it was rather broken.

I've tested this via catkin_make and by calling gradle directly.