ros2 / common_interfaces

A set of packages which contain common interface files (.msg and .srv).
220 stars 106 forks source link

`sensor_msgs_py.point_cloud2` functions treat numpy arrays as having shape (width, height) #236

Open jdlangs opened 8 months ago

jdlangs commented 8 months ago

I'm not sure if this is really a problem or just a different convention but it was something that surprised me that I had to work around. I see that for organized clouds both the read_points and create_cloud functions have logic that treat the input/output ndarray of having a shape where the cloud width is the first index (rows), and the cloud height is the second (columns). I suppose it works if you always follow that convention but I think most other code will treat it with the opposite convention and some axes swapping is necessary.

I think pretty much only the bits I linked would have to be changed to make the switch. @Flova what are your thoughts as the original PR author?

jdlangs commented 7 months ago

So I originally thought this was just a relatively harmless swapping of axes, but now I'm seeing it's creating issues with the point order in organized clouds. Specifically, I'm trying to read point clouds with the following structure:

PointCloud2:
  height:  1544
  width:  2064
  fields:
     sensor_msgs.msg.PointField(name='x', offset=0, datatype=7, count=1)
     sensor_msgs.msg.PointField(name='y', offset=4, datatype=7, count=1)
     sensor_msgs.msg.PointField(name='z', offset=8, datatype=7, count=1)
     sensor_msgs.msg.PointField(name='normal_x', offset=16, datatype=7, count=1)
     sensor_msgs.msg.PointField(name='normal_y', offset=20, datatype=7, count=1)
     sensor_msgs.msg.PointField(name='normal_z', offset=24, datatype=7, count=1)
     sensor_msgs.msg.PointField(name='curvature', offset=32, datatype=7, count=1)
  is_bigend:  False
  is_dense:  True
  point_step:  48
  row_step:  99072
  num_bytes:  152967168

When I call read_points, I get an array of shape (2064,1544) as expected. However, when I try to index the points in a 2D region, I find they are out of order. Switch the argument order in the call to reshape inside read_points fixes this and lets me access a contiguous region of the organized cloud.

mjcarroll commented 7 months ago

I agree that it seems transposed. Would you be able to open a PR with the fix and maybe a unit test so that we can prevent regression in the future?

jdlangs commented 7 months ago

Yes, the fix is simple enough so I'll go ahead with a PR. It will probably be sometime next week.