hector_exploration_planner: Prevent shifting goal pose into unexplored area