Added reachability test
pickup plan only works now