start to replace the position setpoints for waypoint navigation