Fixed up CMake and packge.xml