publishes the filtered data and plan traj works but really slow