Converting PointCloud2 to Structured NumPy array in ROS2

Question

I am trying to convert PointCloud2 data to numpy array.

I am converting it with package: sensor_msgs_py.pointcloud2 using the method: read_points

I always get an error that Cloud is not a sensor_msgs.msg.PointCloud2 but I am sure that the data I am giving to function are PoinCloud2 type. I am getting data directly by subscribing to this topic: /D435_1/depth/color/points [sensor_msgs/msg/PointCloud2] 1 publisher

Camera I am using is: Realsense D435

Code:

from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from std_msgs.msg import Header
import open3d as o3d
import numpy as np
import sensor_msgs_py.point_cloud2 as pc2
import sensor_msgs_py.numpy_compat as nc
import cv2


class MySubscriber(Node):
    def __init__(self):
        super().__init__('my_subscriber')
        self.subscription = None
        self.subscription = self.create_subscription(PointCloud2, '/D435_1/depth/color/points', self.callback, 10)      
        
        self.camera_width = 848
        self.camera_height = 480
        

    def callback(self, msg: PointCloud2):
        self.get_logger().info('Received PointCloud2 message')
        self.point_cloud_data = msg.data
      
           
        points = self.convert_point_cloud_msg_to_numpy(self.point_cloud_data)
        
        
        
        
    def convert_point_cloud_msg_to_numpy(self, data: PointCloud2):
        if self.point_cloud_data is not None:
            
            print('test message 1')
            # Parse the PointCloud2 message
            gen = pc2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
                                
            print('test message 2')


            # Convert the point cloud to a numpy array
            points_numpy = np.array(list(data), dtype=np.float32)

            return points_numpy
            
            
 
       
        

def main():
    rclpy.init()
    subscriber = MySubscriber()
    rclpy.spin(subscriber)
    subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Error message: File "/opt/ros/humble/lib/python3.10/site-packages/sensor_msgs_py/point_cloud2.py", line 88, in read_points assert isinstance(cloud, PointCloud2),
AssertionError: Cloud is not a sensor_msgs.msg.PointCloud2

Answer 1

Figured it out:

you should pass the whole msg data you get from the topic to the read_points function, not just msg.data data.