CFG & generating goal poses were coded. A commented line for debugging was added to CMakeLists.txt