sensor_msgs: Generate optimal msg lentgth when both "xyz" and "rgb(a)…

[tmoinel]

…" are given to PointCloud2Modifier.

With this patch PointCloud2Modifier::setPointCloud2FieldsByString make PointCloud2 message with an optimal length by removing unnecessary trailling offset without messing with the padding. It cut in half the msg length.

When we used something like in https://github.com/ros-perception/image_pipeline/blob/indigo/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp#L251

     sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
     pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); 

By exemple, with kinect images the cloud msg length was 9.83MB. Now it's 4.92MB, like with just the xyz part.

Since it's a core library every project that depends on it should be recompiled.

[tfoote]

@vrabaud can you review?

[vrabaud]

ok, totally agreed in principle (and that was the next step for this PointCloud2 iterator) BUT, by doing your optimization, that would break the PCL converters. Those paddings were added by PCL for having the data-structure memory-aligned, but by consequence, it got the messages to be memory aligned and therefore too big.

There are 3 ways to read PointCloud2:

• convert to PCL: this requires a very specific padding
• deal with field sizes by hand (e.g. in RViz)
• use the Iterator

The idea is to have everybody use the iterator so that we can optimize the size. So the next step, is to have the PCL converters use the iterator but it would require some heavy macro work to get get it to work with any point type.

[tmoinel]

Sorry for the waiting I was in holidays.

ok, I made some test and dig into PCL conversions:

• convert to PCL: this requires a very specific padding
  After some test, a generate PointCloud2 msg from my patch and thanks to PointField definition and FieldMapper in pcl/convertion.h is correctly read from a pointcloud.
  The conversion take multiple memcpy to do instead of one, but after a little test it doesn't affect the perf. Even if it was slower (which is not) network is slower than cpu that's why i think this patch is more important.
  Converting back to PointCloud add the padding you described. Indeed the PCL writer will be much more difficult to optimise. But why other way of writing pointcloud should suffer from that.
• deal with field sizes by hand (e.g. in RViz)
  It's by hand but it use the PointField definition, so it remains correct.
  I can propose a pull request to rvirz to handle the Iterator instead of manually.

If everybody follow the field description there will not be any problems to read. This is, I guess, why PointCloud2 was created for otherwise we would use the old PointCloud.

I also find a little inconsistency in https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h#L216 rgba must be type of uint32 (datatype=6) and not float32 (datatype=7) according to https://github.com/PointCloudLibrary/pcl/blob/master/common/include/pcl/point_types.h#L683 fix by https://github.com/ThomasMoinel/common_msgs/commit/d4778fb06814e550fb51ba5243446d4dc063177f
I can push a fix on a separate pull request if you prefer.

[vrabaud]

For RGBA, please make a pull request quoting:
https://github.com/PointCloudLibrary/pcl/blob/master/common/include/pcl/point_types.h#L108
Thx

[vrabaud]

Forget about my previous comment but please add the commit you mention as a comment so that we keep track of it.

So two things:

• you need to check whether you have XYZ only or RGB/RGBA only. Your current code would removing padding in case of XYZI for example
• at the end of the code, you need to remove something from the offset, not add it.

[vrabaud]

@ThomasMoinel , any input on my questions ? Thx

[peci1]

There is also a relevant effort going on in PCL: PointCloudLibrary/pcl#4939.