[ROS2][feat] Handle various type of points in one converter
Goal
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.
Changes
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.
Results
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 % |
Trajectory
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 measures of time (in sec) for a callback :
- for Velodyne conversion node before !284 (merged).
- for Velodyne conversion node after !284 (merged) and !304 (merged) + !311 (merged) (fix).
- for Generic conversion node after !302 (merged).
- for Generic conversion node after !302 (merged) with forced computations of laser_id and time.
- other specific time measures for code portions, with different conditions (see tab 2 & 3 from section Results)
- The trajectories after running SLAM using
- Velodyne conversion node after !284 (merged) + !304 (merged) + !311 (merged) (fix).
- Generic conversion node after !302 (merged).
- The extracted Slam PointCloud of frame 100 :
- for Velodyne conversion node after !284 (merged) and !304 (merged) + !311 (merged) (fix).
- Raw conversion node (using CallbackXYZI) after !284 (merged) and !304 (merged) + !311 (merged) (fix).
- for Generic conversion node after !302 (merged) with forced computations of laser_id and time.
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.