Updating Main.cpp and CMakeLists.txt for Main Function. Need to fix Ros Publisher stuff