pointcloud2 stream visualization in open3d or other possibility to visualize pointcloud2 in python

Question

I'm trying to visualize a pointcloud2 stream from a rostopic via open3d in python.

This ist my code:

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() 

If I start this code I only get a frozen picture.

I use this script to convert the pointCloud2 to an open3d format.

If someone has another idea to visualize a pointcloud2 in rospy, I would be happy to hear it.

Thanks for help and suggestions!

Answer 1

I figured it out. The rospy.spin() blocks the visualization.

So my final code

Ros node:

if __name__ == '__main__':

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

The 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)

and at least the Viewer Class

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()

I changed also the method to convert the PointCloud2 to an o3d pointcloud