[ROS][feat] Allow to load poses from file

Goal

Allow to load an external trajectory from a CSV file.

These poses can be used to de-skew the input pointcloud, to deduce a prior pose for the registration algorithm and/or to add a constraint in the SLAM optimization.

Changes

• Update the rviz plugin package
• Add a command in command message in lidar_slam package
• Add ext_sensor in TF tree

Results

Reproduce

Data

• Example of CSV file :

ext_sensor 
time,x,y,z,x0,y0,z0,x1,y1,z1,x2,y2,z2 
1695972780,-0.348,0.17,-0.131,-0.974,-0.221,0.012,0.2099,-0.89587,0.392,-0.0755,0.3849,0.9198

Build

catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release

Parameters

• No change

Run

roslaunch lidar_slam slam_velodyne.launch

rostopic pub -1 /slam_command lidar_slam/SlamCommand "{command: 40, string_arg: path/to/ext_poses_ROS.csv}"

Output

You should see :

[ INFO] [1702315740.991950582]: Calibration for ext poses set to : 
1 0 0 0 
0 1 0 0 
0 0 1 0 
0 0 0 1
[ INFO] [1702315740.993038762]: External poses loaded!

Checklist

• Camel case everywhere except for ROS variables/parameters
• Lower case for local variables and lambda functions
• Upper case for pour members, methods and tool functions (in Utils)
• Precise namespace when calling a function (or this->X or classe.X)
• Align code (for multiline if and while, "&&" or "||" go in upper line to ensure alignement)
• Space between if, while, for and parenthesis
• Space between operators and variables: e.g. a + b
• Space after ","
• Mind your commit titles/desc (plurals, he/she + "s", correct tags, title should begin by a verb...)
• Function names should start with a verb, variable names should start with a name
• Macros should be between {}
• Do not use negative boolean (i.e. noJoe)
• Check minimal size of the types (double -> float -> int -> uint)
• Check const and ref in functions arguments
• References should be written "type& name", not "type &name"
• Update documentation if needed
• Add MR labels [ROS]/[ROS2]/[PV]
• If ros/ros2, update task table here
• Add a comment over each non trivial function in header files
• Add a header to each new file