Changes made to the CmakeList so it compiles without having to comment lines from code.
Nevertheless, have to do the catkin_make like 7 or 8 times until everything is compiled wright.