cleaned code for footstep planner node