Added goal finding node and modified cmake and package files to run it