humanoid_planner_2d: check if goal / start reachable before planning