Got partial pointcloud to show up in rviz. Not fully functional yet