added sleep to prevent that the planner updates the trajectory to often near the goal