Skip to content

[ROS2][feat] Handle various type of points in one converter

Jeanne Faure requested to merge ROS2/feat/GeneralizeConverter into feat/ROS2


This MR will create a generic conversion node handling many type of points.

More precisely, it will handle various types, names and combinations of fields.

The goal is to have one converter for all types of drivers, which will recompute missing fields necessary for SLAM execution. We will still keep other converters (the specific ones) which are a bit faster : if the user is certain of the fields of the points he's using, he'll have better performances with the associated specific converter (if existing). However, we'll try to reduce the gap of performances as much as we can between specific converters and the generic one.


In generic_point.h

Creation of a class of various points to be used in the conversion node. They are not directly the points to fill slamPoints with, but points with strictly one field to fill a vector, which will then be used to fill slamPoint.

We had to go through this process as sensor_msgs have various types for PointFields (int8, uint8, ... float32, float64), and we wanted to be able to detect every possible type.

We observed through our multiple LiDAR projects that some point fields don't always have the same name : for Velodyne, the SlamPoint attribute "laser_id" is called "ring". For Ouster, "time" is called "t" and intensity" is "reflectivity" . This can now be handled thanks to distincts type of points. If a new denomination is identified, we shall add a new point to generic_point.h and new "if" in GenericConversionNode.

In GenericConversionNode (former RawToLidarNode)

Here, we grouped the 2 callbacks into 1, as it will handle the possibility of having intensity or not.

The callback first explores every possible name of fields of the msg_received, and its possible type, to store the value in vectors.

Then, the slamPoints are created and filled with x, y , z and the fields encountered in the first loop. The fields missing/incorrect are computed (See this MR for computing of missing fields (laser_id, time, rpm, device_id, nLasers); see fix for time, rpm and device_id ; see other fix for ouster and perf matters).

In Utilities.h

Fix little style issues and add const when possible.


Time taken by conversion node

Principal table : time for a callback in many cases (previous MR, with/without estimations...)

The following table was created with the ROS2 bag bag_ros2-20230829T082957Z-001

The computations were made with the 360 first frames, measuring the time taken by each callback, using only 1 thread, in Release mode.

Velodyne conversion before !284 (merged) (checkout at commit c450dcc53) Velodyne conversion after !284 (merged) & !304 (merged) + !311 (merged) (fix) Raw conversion (CallbackXYZI) after !284 (merged) & !304 (merged) + !311 (merged) (fix) Generic conversion from this MR with fields to find and fill Generic conversion from this MR with forced computations of laser_id and time
AVG 3.23 ms 2.82 ms 6.08 ms 4.18 ms 5.68 ms
STDEV 0.17 ms 0.15 ms 0.32 ms 0.22 ms 0.30 ms

Details on code portions with current changes

We then digged into the "Generic conversion node from this MR with fields to find and fill" to measure different timestamps (see the debug commit associated)

for loop on msg fields with known fields for loop on cloudRaw to fill cloudS without computations
AVG 2.03 ms 0.88 ms
STDEV 0.11 ms 0.046 ms
% in total conversion 49 % 21 %

Finally, we looked at "Generic conversion node from this MR with forced computations of laser_id and time" (see the debug commit associated)

for loop on msg fields with unknown fields for loop on cloudRaw to fill cloudS with computations
AVG 0.74 ms 3.83 ms
STDEV 0.039 ms 0.20 ms
% int total conversion 13 % 67 %


We wanted to measure the impact of laser_id and time computations on the trajectory. See below how close the trajectories happen to be. We ran SLAM on the same velodyne bag used above, firstly using velodyne conversion node, then generic conversion node artificially asking for computation of laser_id and time. We used CloudCompare to display the following results.

Both trajectories superimposed : the red one obtained with velodyne node, the green one with generic node with forced computations of laser_id and time


Zoom on first and last turn (the last being above the first)


Sheet to test by yourself

You'll find in this google sheet :

The only bag used for all these tests is bag_ros2-20230829T082957Z-001

Note : !284 (merged) was merged, then !304 (merged), then !311 (merged). This MR ( !302 (merged)) is rebased on !321 (merged) commits.

Edited by Jeanne Faure

Merge request reports