Added detector helper function file

Added obstacle detection functions including graham scan.