Визуализация потока pointcloud2 в open3d или другая возможность визуализировать pointcloud2 в python

Question

Я пытаюсь визуализировать поток pointcloud2 из rostopic через open3d в python.

Это мой код:

import sensor_msgs.point_cloud2 as pc2
import open3d
...


def callback(self, points):
        #self.pc = pcl.VoxelGridFilter(self.pc)

        self.pc = self.convertCloudFromRosToOpen3d(points)

        if self.first:
            self.first = False
            self.vis.create_window()
            rospy.loginfo('plot')
            self.vis.add_geometry(self.pc)
            self.vis.update_geometry()
            self.vis.poll_events()
            self.vis.update_renderer()
            self.vis.run()
        else:
            rospy.loginfo('update')
            self.vis.update_geometry()
            self.vis.poll_events()
            self.vis.update_renderer()
            self.vis.run()

    def listener(self):
        rospy.init_node('ui_config_node', anonymous=True)
        rospy.Subscriber('/kinect2/sd/points', PointCloud2, self.callback)
        rospy.spin() 

Если я запускаю этот код, я получаю только замороженное изображение.

Я использую этот скрипт для преобразования pointCloud2 в формат open3d.

Если у кого-то есть еще одна идея визуализировать pointcloud2 в rospy, я был бы рад ее услышать.

Спасибо за помощь и предложения!

Answer 1

Я понял. Вrospy.spin() блокирует визуализацию.

Итак, мой последний код

Узел Ros:

if __name__ == '__main__':

    listener = CameraListner()
    updater = Viewer(listener)
    rospy.spin()

CameraListner

class CameraListner():
    def __init__(self):
        self.pc = None
        self.n = 0
        self.listener()

    ############################################################################
    def callback(self, points):
        self.pc = points
        self.n = self.n + 1

    def listener(self):
        rospy.init_node('ui_config_node', anonymous=True)
        rospy.Subscriber('/kinect2/sd/points', PointCloud2, self.callback)

и хотя бы класс Viewer

class ViewerWidget(QtWidgets.QWidget):
    def __init__(self, subscriber, parent=None):
        self.subscriber = subscriber
        rospy.loginfo('initialization')

        self.vis = open3d.visualization.Visualizer()
        self.point_cloud = None
        self.updater()

    ############################################################################
    def updater(self):

        rospy.loginfo('start')
        self.first = False
        while (self.subscriber.pc is None):
            time.sleep(2)
        self.point_cloud = open3d.geometry.PointCloud()
        self.point_cloud.points = open3d.utility.Vector3dVector(ros_numpy.point_cloud2.pointcloud2_to_xyz_array(self.subscriber.pc))
        self.vis.create_window()
        print('get points')
        self.vis.add_geometry(self.point_cloud)
        print ('add points')
        self.vis.update_geometry()
        self.vis.poll_events()

        self.vis.update_renderer()

        while not rospy.is_shutdown():
            self.point_cloud.points =  open3d.utility.Vector3dVector(ros_numpy.point_cloud2.pointcloud2_to_xyz_array(self.subscriber.pc))
            self.vis.update_geometry()
            self.vis.poll_events()
            self.vis.update_renderer()

Я также изменил метод преобразования PointCloud2 в o3d pointcloud