Simplifying everything, adding CMakeLists.txt and package.xml