Fix for some issues in CMakeLists.txt and package.xml