ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org

Proposal for a new PointCloud2Iterator & PointCloud2Modifier

This pose is here in support for a PR: Add point cloud message wrapper by niosus · Pull Request #141 · ros2/common_interfaces · GitHub

The main goal is to have a broader discussion if this proposal would be perceived as relevant and to discuss possible ways to improve it should it not seem relevant.

So let me start by discussing why we developed a new way to work with point cloud 2 messages. I believe it is best illustrated by an example.

sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier mod{cloud};
mod.resize(points.size());
mod.setPointCloud2Fields(
  2,
  "x", 1, sensor_msgs::msg::PointField::FLOAT32,
  "y", 1, sensor_msgs::msg::PointField::FLOAT32);
sensor_msgs::PointCloud2Iterator<float> write_x_iterator{cloud, "x"};
sensor_msgs::PointCloud2Iterator<float> write_y_iterator{cloud, "y"};
for (std::size_t idx = 0U; idx < points.size(); ++idx) {
  *write_x_iterator = points[idx].x;
  *write_y_iterator = points[idx].y;
  ++write_x_iterator;
  ++write_y_iterator;
}
// This can happen i a different part of the program
sensor_msgs::PointCloud2Iterator<float> it_x{cloud, "x"};
sensor_msgs::PointCloud2Iterator<float> it_y{cloud, "y"};
for (; it_x != it_x.end(); ++it_x, ++it_y) {
  std::cout << *it_x << " " << *it_y << std::endl;
}

Here we create a point cloud message, initialize it to hold "x" and "y" floating point fields and fill it with some points array. Then we read these points and print them to the console. AFAIK, this is the shortest syntax to do this. There are some things here that arguable could be improved:

  • The message must be resized to the correct size before use
  • All the fields must be configured manually
  • The iterators must have the correct underlying type (float) and the correct field name. It is a responsibility of the user to make sure these are correct. This is ok when reading and writing the message happens within a single scope like in the example above, but it can happen in different part of the programs or even on different machines. Failing to provide the correct type causes undefined behavior, which was the initial reason to start rethinking this concept
  • The code is slightly verbose and the more iterators we need the more verbose it gets.

The code proposed in the PR referenced at the start of this post would allow for the following syntax:

sensor_msgs::PointCloud2 cloud;
PointCloud2Modifier<PointXY> modifier{cloud};
for (const auto& point : points) {
  modifier.push_back(point);
}
// This can happen in a different part of the program
PointCloud2View<PointXY> view{cloud};
for (const auto& point : view) {
  std::cout << point.x << " " << point.y << std::endl;
}

Note that there are the following additional benfits:

  • No need to manually configure iterators/modifiers - the fields are generated automagically from the PointXY type (stay tuned)
  • No way to make a mistake with the size of the cloud - modifier grows the point cloud when he points are being pushed into it (but a reserve or a resize is possible)
  • If a wrong type would be provided to the PointCloud2View the code would throw. This check does cost some time but only upon creation of a view or a modifier
  • There is no performance cost for reading/writing points
  • The code is clear and consice

Now, hopefully I could convince you of the usefullness of this approach and we can talk about how this is implemented and discuss the trade-offs that come with this code.

As C++ has no reflection, the only way to “automagically” generate the fields vector out of a provided point type (PointXY in an example above) is to have pre-defined traits for that. As part of the PR we provide a macro that generates traits for checking if a type has a member or a method with a given name as well as a class that is able to generate a valid PointField with this specified name that can be put into the vector of such fields inside of the message. By default we pre-generate a number of these generators (field_x_generator, field_y_generator, field_intensity_generator, etc.) so for points like PointXY, PointXYZ, PointXYZI the code will work out of the box. Power users can extend these generators with their own (see unit tests for an example of this).

When the message wrapper (either a modifier or a view) is created, we perform a check if:

  • the fields generated from the type of the point provided to the wrapper matches the fields already stored in the message. We fail early if they don’t match indicating the problematic field to the user.
  • when creating a new message we check if all the members of the provided point type are covered by the field generators that the wrapper is aware of. If not, we fail early and indicate to the user that there are missing field generators

These checks cost some time but are performed only upon the wrapper creation. After that the data can be safely reinterpreted as the provided point type.

That brings us to the last trade-off of this approach. The data is directly mapped to the message, that means that if there are paddings in the point type - these will also be stored in the point cloud potentially wasting some space. The benefit here is that then the data is always correctly aligned. It might cause some issues of not being able to read a message with the proposed classes on a different platform (due to different padding). However, we believe that in such a situation care must anyway be taken and a more manual approach should anyway be used in such cases.

Anyway, this post is long enough already and I believe it gives a good enough high-level overview of the problem we are trying to sove and the currently proposed solution. I am absolutely welcoming any suggestions and/or discussion points!

8 Likes

I’ve been looking for something like this for some time. A similar effort was undertaken by @mfc as part of our current development efforts for Autoware.Auto (WIP MR here) but I would definitely prefer to see this in he upstream package. Perhaps @mfc could review this MR and make suggestions regarding the implementation from his experience?

1 Like

Speaking from experience there was a big learning curve for me with regards to writing cpp code to deal with PointCloud2 messages. This MR seems to make that learning curve a lot smaller…looks promising!

Agreed, I’d love to see this! PointCloud2’s I think are a bit unnecessarily hard to work with, especially if you don’t work in C++ daily. Most of the time I convert to a PCL pointcloud to work with, but I’d be alot more compelled to work directly with message types if the code was more readable like your suggestion. Readability is very important to me more than performance (to a limit) to make sure its long-term maintainable and understandable to the largest cross section of people to be able to contribute.

Edit: I might suggest it take on another name from the existing class names since it would be a convenience util that works with the most common types of points (since the padding / types discussion above make it not truly fully general purpose). SimplePointCloud2Modifier or something.

3 Likes

Thanks guys, I’m happy that the ideas are perceived as needed in the community!

@smac I’m very open to different names, having ones that make sense to everyone is important!

Unfortunately it is not going to be possible to fully remove the complexity from the user side as the PointCloud2 is essentially a very low-level type. So we can hide the complexity behind the wall but as soon as the user would need to use more than we show in the “simple” part, they need to be able to understand the guts, which are not trivial and require significant C++ knowledge.

As for the “not being able to use with every type”. I’ve been thinking about it more especially after seeing the approach @mfc took (very related to this proposal). I think it is fair to say that we can use this in all situations with the caveat being that the point structure/class must correspond exactly to the one written into the message. We can do that by using alignas or similar machinery in C++. It is still not really portable, but should satisfy most of the use-cases.

Also, there is very marginal performance hit that has to be payed in terms of small (approximately constant time) runtime penalty when creating the wrapper. All the access to the point cloud data has no overhead.

1 Like

I haven’t looked at the code of the PR, but the example you posted is really encouraging. Having something like that would definitely help keeping code short and readable.

Is the main motivation for this (ie: directly working with messages as data structures) to avoid overhead from converting to pcl:: types?

@gavanderhoorn yep, that would be the use case.

It seems like you’ve received very positive feedback @niosus, any word on bringing this to the world?

1 Like

@smac I guess the current decision was that this is too early to push this into the ros2 core repo and it should start it’s life in a separate one. This will happen some time this or next week and once (and if) it gets traction we can move it into the core.

Let me know! I’d love to use it in Nav2 when its ready

2 Likes

@smac the code is now available to try out here: ApexAI / Point Cloud Message Wrapper · GitLab

I will also soon release it through rosdistro for convenience. Please do give it a try and open issues for anything that is unclear or too hard to use :wink:

Awesome, might I suggest 3 things

  • A better name? This is unclear what the package does to me (point_cloud_msg_wrapper), I think you can come up with something more slick like simple_pointcloud2_modifier or something :slight_smile:
  • Example usage: I often look at tests for example usages but I don’t see any from a user’s perspective. It might be good to have some integration tests for people to base their code off of or a simple demo or two using standard point types like XYZ / XYZRGB. Even better if there was a stand-alone demo showcasing how to create another, simple PointT like PointABC to show in a concise way the minimum of what needs to be created to support any other point type this package doesn’t natively support. Basically, just usage examples for users.
  • In the readme, explicitly mention which point templates you already have supported so a user knows what’s batteries included

Love this work, hopefully we can make use of this in Nav2 and that helps getting this into sensor_msgs! The most obvious places to use this in Nav2 is the observation buffer and the marking / raycasting in Voxel and Obstacle layers.

Sounds good! Thanks for the suggestions. I will definitely brainstorm the name, but everything else might be needing to be added in the future purely due to time reasons :man_shrugging: I was hoping that the tests were kinda clear, but I might simplify and rearrange them a bit for simplicity.

1 Like

@smac I cannot think of anything better considering the name for now. It is not really a modifier as it also provides a const view (PointCloud2View), which is a separate class.

I think to get this going, I will release it by its current name and if the code is any good/useful I hope it will still get used. If there are any last minute naming suggestions, they are still welcome of course.

Just a small update here, the package is now available through ROS2 build farm, check it out here: ROS packages for Foxy - 2021-07-12 02:57:03 -0800

Sorry @smac I did not have the time to work on it much more to rethink a name and to improve the docs by a lot, but I hope it will be enough for a start.

1 Like