footstep_planner: Merge plan and replan functions