Added goal finding node and modified cmake and package files to run it
merge changes
Merge branch 'valeRB-master'
Merge pull request #14 from valeRB/master

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