Fixed plane finder. Add plane points publisher (for debugging)