add pathTracker; add orientation to goal; add safety distance around goal; add angle helpers