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
PointXYtype (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
reserveor aresizeis possible) - If a wrong type would be provided to the
PointCloud2Viewthe 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!
I was hoping that the tests were kinda clear, but I might simplify and rearrange them a bit for simplicity.