sketched out algorithm for picking goal points
Merge branch 'master' of github.com:gtagency/ros-self-driving-core