Reupload deleted CMakeFiles and package.xml
Code clean up