Updated package.xml and cleaned up CMakeLists.txt