Cleaner code
Added header files. stupid workaround until i get catkin to work