Open3d Visualizer not Displaying Pointcloud From Ros Topic

asked 2023-05-26 23:58:47 -0500

hck007 gravatar image

Problem: I'm trying to visualize real-time point cloud data using Open3D in a ROS (Noetic) environment and Ubuntu 20.04. I've subscribed to a point cloud topic and convert the data into an Open3D point cloud format, then attempt to visualize it using Open3D's Visualizer. However, the Open3D window remains empty with no point cloud visible. The error is [Open3D WARNING] GLFW Error: GLX: Failed to make context current. The following is my code.

import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import open3d as o3d
import numpy as np

class PointCloudVisualizer:
    def __init__(self, topic):
        self.point_cloud_o3d = o3d.geometry.PointCloud()
        self.vis = o3d.visualization.Visualizer()
        self.vis.create_window()
        self.vis.add_geometry(self.point_cloud_o3d)
        self.sub = rospy.Subscriber(topic, PointCloud2, self.callback)

    def callback(self, msg):
        # Convert the ROS point cloud message to a numpy array
        cloud_arr = np.array(
            list(pc2.read_points(
                msg, field_names=("x", "y", "z"), skip_nans=True)))

        # Update the point cloud for Open3D
        self.point_cloud_o3d.points = o3d.utility.Vector3dVector(cloud_arr)

        # Update the visualization
        self.vis.update_geometry(self.point_cloud_o3d)
        self.vis.poll_events()
        self.vis.update_renderer()

if __name__ == "__main__":
    rospy.init_node('point_cloud_visualizer')

    # Specify your point cloud topic
    point_cloud_topic = "/your/pointcloud/topic"

    vis = PointCloudVisualizer(point_cloud_topic)

    # Keep the node running until it's shut down
    rospy.spin()
```

Attempt: I searched the Internet, and find out someone has exactly the same problem with me. it looks like open3d uses a thread that is not allowed to be used in callback function of ROS. Does anyone have any solution? If no, any work around for the bug?

edit retag flag offensive close merge delete