1

我正在尝试通过 python 中的 open3d 可视化来自 rostopic 的 pointcloud2 流。

这是我的代码：

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 格式。

如果有人有另一个想法来可视化 rospy 中的 pointcloud2，我会很高兴听到它。

感谢您的帮助和建议！

4

1 回答 1

2

我想到了。rospy.spin()块可视化。

所以我的最终代码

罗斯节点：

if __name__ == '__main__':

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

相机监听器

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)

至少是查看器类

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 点云的方法

于 2019-11-25T13:20:22.237 回答