ROS 2 Python PointCloud2 to xyz NumPy array: read_points_numpy

한국어로 읽기

What happened when I ran it

The question comes up again and again: ROS 1 had sensor_msgs.point_cloud2.read_points(), so what do you call in a ROS 2 Python node to turn a sensor_msgs/PointCloud2 into an (N, 3) NumPy array of x, y, z? The short answer is read_points_numpy from the sensor_msgs_py package. I wanted to know whether you can trust its output, and what it does with NaNs, mixed field types and organized clouds.

So I built a fake depth-camera cloud in a ROS 2 Jazzy container: 640×480 points, x/y/z as float32, 16 bytes per point, with 40,000 randomly chosen points made invalid (NaN). A small rclpy node published it on a topic and received it back. Every measurement below was taken on the received message.

The basic conversion was correct. read_points_numpy returned a float32 array of shape (307200, 3), and its z column matched the bytes read by hand with struct at every single point. The surprise was skip_nans=True. With is_dense=False it removed all 40,000 NaN rows. When I set is_dense=True on the same data, it removed none of them, with no warning. Four other details are not in the API docs either: which field the type check uses as its reference, inf values, reshape_organized_cloud, and uvs. Each of them returned something different from what most people would expect.

The short answer

import numpy as np
from sensor_msgs_py import point_cloud2

def callback(msg):
    xyz = point_cloud2.read_points_numpy(
        msg, field_names=['x', 'y', 'z'], skip_nans=True)
    # xyz.shape == (N, 3), dtype float32 for float32 fields

This works on Jazzy when the first field in msg.fields has the same type as x, y and z, which is the usual layout. Before you rely on it, check three things:

  • NaNs. skip_nans only filters when msg.is_dense is False, and it never removes inf. A plain mask, xyz[np.isfinite(xyz).all(axis=1)], does not depend on either.
  • Field types. If you get AssertionError: All fields need to have the same datatype. Use `read_points()` otherwise., use structured_to_unstructured(read_points(...)) instead (see below). On Humble you will hit this error more often.
  • Row padding. If msg.row_step != msg.width * msg.point_step, the read_points* functions read the wrong bytes from the second row on.

Measured results

Environment. Image ros:jazzy-ros-base (ros@sha256:066420e07f60aa18262f2479981def87ebcfcec42eefb0c0c57c4a46098348ca), ROS 2 Jazzy, sensor_msgs_py 5.3.8, numpy 1.26.4, Python 3.12.3, rclpy 7.1.12-1noble.20260902.053513, RMW rmw_fastrtps_cpp (default), little-endian, no network, limits 1 CPU and 1 GiB. The script experiment.py (sha256 97822524b3e5678fd4ea5c38a38f1f4cab20ef4fa823a4564954c9bc8d6ea394) ran 5 times. All 5 runs exited with code 0 in 1.52 s to 2.87 s, with empty stderr. In every run the cloud arrived after 1 publish (0.139 s to 0.278 s after start). Apart from those timings, every value below was identical in all 5 runs.

Input. Organized cloud, width 640, height 480, fields x/y/z at offsets 0/4/8 (datatype 7, FLOAT32), point_step 16, row_step 10240. 40000 points (seed 23782) got a NaN in one of x, y or z. The lidar-style clouds have 16 points. The other edge cases use tiny clouds (5 points, or height 2 × width 3). The predictions come from reading the Jazzy source and were written in plan.md before the runs.

Check Predicted (plan.md) Measured
Points = width × height 307200 307200; len(data) 4915200, equals width × height × point_step: true
Byte where point 1000’s z starts 16008 (row 1, col 360) 16008, row 1, col 360; struct z 1.0299999713897705 = read_points_numpy z 1.0299999713897705
struct z vs read_points_numpy z, all points 0 mismatches 307200 compared, 0 mismatches
xyz array (307200, 3), 3686400 bytes (307200, 3), float32, 3686400 bytes
field_names=['z','x'] (column order checked) columns come back as z, then x: true
skip_nans=True, is_dense=False 267200 rows, 0 with NaN 267200 rows, 0 with NaN
skip_nans=True, is_dense=True, same data 307200 rows, 40000 with NaN 307200 rows, 40000 with NaN
np.isfinite mask on the same data — 267200 rows
A: [x, y, z, intensity f32, ring u16], select ['x','y','z'] passes, (16, 3) float32 passes, (16, 3) float32
Same lidar layout, ring of point 10 starts at byte 336 byte 336; struct 102 = read_points 102
B: same layout, field_names=None or ['x','ring'] AssertionError AssertionError in both cases
C: [ring u16, x, y, z f32], select ['x','y','z'] read_points_numpy AssertionError; read_points + structured_to_unstructured (16, 3) AssertionError; workaround (16, 3) float32
5 points: 2 rows with inf, 1 with NaN, skip_nans=True 4 rows left, 2 with inf; isfinite mask 2 4 rows, 2 with inf; isfinite mask 2 rows
read_points(..., reshape_organized_cloud=True), height 2 × width 3 shape (3, 2), “rows” not image rows shape (3, 2); 1 “row” spans two image rows. Manual reshape(height, width, 3): (2, 3, 3), 0 rows span two image rows
Same, with a NaN and skip_nans=True ValueError ValueError: cannot reshape array of size 5 into shape (3,2)
Point 0 has NaN x, uvs=[0] skip_nans=True returns original point 1; False returns point 0 original index 1 with skip_nans=True, 0 with skip_nans=False
width 3, height 2, row_step 56 (8 padding bytes per row) second row’s first x read wrongly (true value 10) true x 10.0, read_points_numpy x 0.0; real start byte 56, assumed byte 48; 1 row mismatched; no exception
data 16 bytes shorter than width × height × point_step TypeError data 80, needed 96: TypeError buffer is too small for requested array

Raw lines from run 1 (stdout, one JSON object per measurement):

{"measure": "skip_nans_vs_is_dense", "input_rows": 307200, "input_rows_with_nan": 40000, "is_dense_false": {"rows": 267200, "rows_with_nan": 0}, "is_dense_true": {"rows": 307200, "rows_with_nan": 40000}, "manual_isfinite_rows": 267200}
{"measure": "type_check_C_first_u16_select_xyz", "read_points_numpy": {"ok": false, "error": "AssertionError", "message": "All fields need to have the same datatype. Use `read_points()` otherwise."}, "read_points_plus_structured_to_unstructured": {"ok": true, "shape": [16, 3], "dtype": "float32"}}
{"measure": "reshape_organized", "height": 2, "width": 3, "shape": [3, 2], "rows_col_row": [[[0, 0], [1, 0]], [[2, 0], [0, 1]], [[1, 1], [2, 1]]], "rows_spanning_two_image_rows": 1, "manual_reshape_shape": [2, 3, 3], "manual_rows_spanning_two_image_rows": 0}
{"measure": "row_step_padding", "width": 3, "height": 2, "point_step": 16, "row_step": 56, "data_len": 112, "width_x_height_x_step": 96, "row1_col0_true_x": 10.0, "row1_col0_read_points_numpy_x": 0.0, "row1_col0_byte_by_row_step": 56, "row1_col0_byte_assumed": 48, "rows_mismatched": 1}

Every prediction held. That is the expected result, because the predictions came from reading the same source file that ran. What the runs add is confirmation on a real message that went through rclpy and DDS (the received data was a Python array, and read_points_numpy accepted it as is). They also show that the problem cases fail in the way described below: silently, or with the exact exception shown.

How the bytes are laid out: point_step and offset

A PointCloud2 carries its points as one flat byte array, data. The rest of the message describes how to cut it up (PointCloud2.msg):

  • height × width: an organized cloud (from a depth camera, for example) has height rows and width columns, like an image. An unordered cloud has height = 1 and width = number of points.
  • point_step: bytes per point. row_step: bytes per row.
  • fields: one PointField per channel, with its name, its byte offset from the start of the point, a datatype code (7 is FLOAT32, 4 is UINT16) and a count.
  • is_dense: “True if there are no invalid points”.

From these, three formulas give you everything you need:

  • number of points = width * height
  • bytes needed = width * height * point_step (when rows have no padding)
  • field f of point i starts at byte i * point_step + f.offset

Here they are on the test cloud. 640 × 480 = 307,200 points. With a 16-byte point, data is 307,200 × 16 = 4,915,200 bytes. Only 12 of each 16 bytes hold x, y and z, so 307,200 × 4 = 1,228,800 bytes are padding. Point 1000 sits at row 1, column 360 (1000 = 640 + 360), and its z starts at byte 1000 × 16 + 8 = 16,008. You can get the same number by going row by row: row_step * 1 + 360 * 16 + 8 = 10,240 + 5,760 + 8 = 16,008. The output array has 307,200 × 3 float32 values, which is 3,686,400 bytes, so a full organized frame costs about 3.7 MB once converted. For the lidar-style layout used in the test (32-byte points, a ring field at offset 16), point 10’s ring starts at byte 10 × 32 + 16 = 336.

read_points_numpy does exactly this, without a Python loop. read_points builds a NumPy structured dtype whose itemsize is point_step and whose field offsets are the PointField offsets (dtype_from_fields, L190–L235). It then views data as width * height elements of that dtype (L84–L87), selects the requested fields in the order you gave (L90–L94), and read_points_numpy flattens the result into a plain 2-D array with structured_to_unstructured (L150–L155). That is why the column order follows field_names: ['z','x'] gives z first.

The three functions differ only in what they return. read_points returns a structured array. read_points_numpy returns a 2-D array, and all selected fields must share one type. read_points_list returns a list of namedtuples. The often-quoted Stack Exchange answer does np.array(read_points_list(...)). That builds a Python list of tuples and then turns it back into an array, so you can skip the detour.

skip_nans only works when is_dense is False

The Jazzy API docs say skip_nans means “don’t return any point with a NaN value”. The source adds a condition that the docs leave out (L101–L109):

if skip_nans and not cloud.is_dense:
    ...  # drop points where any selected field is NaN

This is a reasonable optimisation. is_dense=True promises there are no invalid points, so there is nothing to filter. The trouble starts when the publisher sets is_dense=True and still sends NaNs. Then your array keeps every NaN row, which is what the test showed: 40,000 of them. I did not test real drivers, so I can’t say how often that happens. The point is that skip_nans trusts the flag and never checks the data.

Two more details:

  • Only NaN is checked (np.isnan). +inf and -inf pass through, as the 5-point test showed.
  • ROS 1’s read_points ignored is_dense and tested every point with math.isnan (ROS 1 source, L81–L113). So ROS 1 code ported line for line can give different row counts on the same message.

If NaN or inf points matter to you, don’t depend on the flag. Mask the data yourself:

xyz = point_cloud2.read_points_numpy(msg, field_names=['x', 'y', 'z'])
xyz = xyz[np.isfinite(xyz).all(axis=1)]

On the test cloud this gave the same 267,200 rows as skip_nans with a correct is_dense=False, and it removes inf as well.

“All fields need to have the same datatype”: the check uses fields[0]

read_points_numpy can only return a single dtype, so it asserts that the fields match first. In Jazzy the check is (L150–L152):

assert all(cloud.fields[0].datatype == field.datatype
           for field in cloud.fields[1:]
           if field_names is None or field.name in field_names), \
    'All fields need to have the same datatype. Use `read_points()` otherwise.'

It does not compare the selected fields with each other. It compares each selected field with fields[0], whether or not you selected fields[0]. In practice:

  • Layout [x, y, z, intensity, ring] (x first, ring is uint16) with ['x','y','z']: passes, because x, y and z all match fields[0] = x.
  • Same layout, all fields or ['x','ring']: fails, as it should, because ring is uint16.
  • Layout [ring, x, y, z] with ['x','y','z']: fails even though all three selected fields are float32, because the reference is ring.

The fix for the last case, and for Humble in general, is to select the fields with read_points and flatten them yourself:

from numpy.lib import recfunctions as rfn

pts = point_cloud2.read_points(msg, field_names=['x', 'y', 'z'], skip_nans=False)
xyz = rfn.structured_to_unstructured(pts)   # (N, 3) float32 in the test

The Jazzy check, which respects field_names, came in with commit 10880a5 “Fix read_points_numpy field_names parameter” (May 2023). Humble 4.9.2 still has the older form, which compares every field in the cloud with fields[0] and ignores field_names (Humble source, L158–L159). So on Humble, any cloud with a uint16 ring or a uint32 rgb field fails, even for ['x','y','z']. I only read the Humble source; I did not run it.

A side note: this is an assert, so running Python with -O removes the check entirely. Normal ros2 run does not use -O.

Row padding, reshape_organized_cloud and uvs all work on flat indices

All three of these surprises come from one design choice. read_points treats the cloud as one flat run of width * height points.

row_step is never used. If a publisher pads each row (row_step > width * point_step), the second row onward is read from the wrong bytes. In the test the second row was read starting at byte 48 instead of 56, so the first x of that row came back as 0.0 (padding) instead of 10.0. No exception was raised, because data was longer than needed. Only when data is shorter than width * height * point_step do you get TypeError: buffer is too small for requested array. ROS 1 advanced by row_step per row, so it handled padding. I have not checked which real drivers pad rows. Checking row_step == width * point_step costs one line.

reshape_organized_cloud=True produces (width, height), not (height, width). The source does points.reshape(cloud.width, cloud.height) (L120–L121). A reshape keeps the memory order and only changes the shape, so the result is not a transposed image. Its “rows” simply don’t line up with image rows. In the 2 × 3 test, the middle “row” held the last pixel of image row 0 and the first pixel of image row 1. This is tracked in common_interfaces issue #236, still open as of 2026-10-13, where a maintainer replied that it “seems transposed”. If you need an image-shaped array, take the flat result and reshape it yourself, before removing any points:

xyz = point_cloud2.read_points_numpy(msg, field_names=['x', 'y', 'z'])
img = xyz.reshape(msg.height, msg.width, 3)   # img[v, u] is pixel (u, v)
valid = np.isfinite(img).all(axis=2)          # keep the mask, not a shorter array

Combining skip_nans=True with reshape_organized_cloud=True fails whenever a NaN was removed, because the point count no longer fits the shape (ValueError: cannot reshape array of size 5 into shape (3,2) in the test).

uvs indexes the array after NaNs are removed. In ROS 1, uvs was a list of (u, v) pixel pairs. In Jazzy it is a list of flat indices, and it is applied after the NaN filter (L112–L117). If point 0 is NaN, uvs=[0] with skip_nans=True returns what was originally point 1. If you need to map points back to pixels, don’t combine the two. Use the full array plus a validity mask, as in the snippet above, and index it with v * width + u.

What to print first, and which call to use

When a cloud from a new driver looks wrong, print its layout once before writing any conversion code:

self.get_logger().info(
    f'{msg.height=} {msg.width=} {msg.point_step=} {msg.row_step=} '
    f'{msg.is_dense=} {msg.is_bigendian=} '
    f'{[(f.name, f.offset, f.datatype, f.count) for f in msg.fields]}')

Then decide:

  1. row_step != width * point_step? Don’t trust any read_points* result. Slice each row by row_step yourself first.
  2. Jazzy or newer, and fields[0] has the same type as x, y, z? Use read_points_numpy(msg, field_names=['x','y','z']).
  3. Humble, or a layout where fields[0] is something like a uint16 ring? Use structured_to_unstructured(read_points(msg, field_names=['x','y','z'])).
  4. Invalid points. If is_dense is False and inf doesn’t matter, skip_nans=True is fine. Otherwise, or if you are not sure the flag is honest, filter with np.isfinite.
  5. Need the image shape or pixel positions? Don’t use reshape_organized_cloud, skip_nans or uvs. Reshape the flat array to (height, width, 3) and keep a mask.

The ros2_numpy package is an alternative. Its pointcloud2_to_xyz_array(..., remove_nans=True) masks with np.isfinite regardless of is_dense, and its numpify reshapes to (height, width). It is a separate install, though, and converts to float64 by default. I read its code but did not run it here.

Limits of this test

  • All clouds were synthetic and built for this test. No real depth camera, lidar driver or robot was involved. Nothing here says how any particular driver fills is_dense, row_step or its field order. The article only covers reading sensor data, not moving hardware.
  • Only ROS 2 Jazzy (sensor_msgs_py 5.3.8) was run. Humble behaviour comes from reading the 4.9.2 source. For Kilted and Rolling I only checked that the relevant lines (the is_dense condition, the fields[0] comparison, the (width, height) reshape) match Jazzy as of 2026-10-13. If issue #236 is fixed, the reshape section will change.
  • Only little-endian data was tested, and no fields with count > 1.
  • No speed or memory benchmark was done. The run timings only show that the script finished.

Sources and reproduction

The full reproduction is at github.com/sungpyo9053/blog/…/ros2-python-pointcloud2-to-xyz-array-read-points-numpy. It contains experiment.py, the predictions in plan.md, and the raw output of all five runs in results.json. The byte-offset arithmetic is checked separately in example.py and verification.json.