Cleaned up CMakeLists and package.xml of assignment_1
Solved the first assignment