Cleaned up CMakeLists.txt and package.xml files