[ROS2][feat] Autocompute laser_id and time

Goal

We aim to simplify the user experience. This MR wants to minimize to the fullest extent the number of fields asked to the user when launching the SLAM.

Before, we used to ask for laser_id and time of each point, as well as Rpm and the nbLasers to make computations and device_id of the device.

The goal of this MR is to compute all these parameters thanks to space coordinates only and PCLHeader data.

In a future MR, we will generalize the minimalist node RawToLidarNode to be used as the only conversion node, for various type of points with various field names and types.

Changes

In SpinningSensorKeypointsExtractor

• Allow the keypoints extractor to handle random laser_id (not necessarily sequenced and not necessarily starting at 0)

In RawToLidarNode

• Create a new ROS2 node RawToLidarNode handling two type of points :
  • One callback for points with only space coordinates (x, y and z).
  • Another callback for points with space coordinates and an intensity field.
• Get the height field of the PCLHeader if correctly set to be used as nbLasers.

In Utilities.h

• Estimate laser_id :
  • Cluster vertical angles into nbLasers categories.
  • Deduce the laser_id of each point by assigning its vertical angle to the correct cluster.
• Estimate time :
  • Deduce rotation sense of the lidar to correctly orient the azimuth (called ClockwiseRotationBool to be used as a boolean parameter).
  • Estimate time using the azimuth angle.
• Create useful initialization functions for all types of Callbacks :
  • Initialize the PCL PointCloud
  • Initialize the SLAM PointCloud
  • Initialize useful parameters to estimate time and laser_id : rotation sense and clusters
• Estimate Rpm :
  • Get timestamp field of the PCLHeader and compute the difference between the previous one
  • Recompute RPM at each frame to refine
  • Exclude outliers thanks to a const map of possible frequency values specific to each LiDAR
• Get DeviceId :
  • Get frame_id field of the PCLHeader
  • Use a map with keys of frame_id and values of DeviceId, of length of the number of devices connected

In other conversion nodes

• This MR also includes changes in OusterToLidarNode, VelodyneToLidarNode, RobosenseToLidarNode :
  • Check the validity of time
  • Add time and/or laser_id estimations if needed
  • Add initialization functions for callbacks
  • Remove LaserIdMapping parameter, because useless in our current implementation
  • Get the height field of the PCLHeader if correctly set to be used as nbLasers
  • Deduce device_id and Rpm from PCLHeader fields
• This MR also changes LivoxToLidarNode :
  • Deduce device_id from PCLHeader frame_id field
  • No other changes because this node is quite particular. Maybe to revise in a future MR.

Results

Commands to reproduce the result

ros2 bag play path/to/this_bag.bag --remap /topic_to_remap:=/xyzi_lidar_points [or :=/xyz_lidar_points]

To run the node (cf 1st video below) : ros2 run lidar_conversions raw_conversion_node

To run SLAM (cf 2nd video below) : ros2 launch lidar_slam slam_velodyne.py use_sim_time:=false aggregate:=true
if line 103 of slam_velodyne.py has been changed from executable="velodyne_conversion_node" to executable="raw_conversion_node"

Results

Video of raw node running : https://drive.google.com/file/d/1jZofw_qM6VUTNX2J3SK41sGh5eLH7Nae/view?usp=sharing

Video of the SLAM running with raw conversion node : https://drive.google.com/file/d/1wK2LBuqII9wZBP00iNJPLTiUEoSgNFso/view?usp=sharing

What remains to solve

In a following MR

• Create various type of points for every combination of field name and field type.
• Modify RawToLidarNode :
  • Create a unique Callback
  • In this Callback, analyze every field of the pcl message to use the right type of points, then estimate the missing fields
• Redirect the conversion step from every launchfile towards the raw_conversion_node.