[fix][ROS2] Fix pose integration and calibration

Goal

This MR brings changes from ROS1 to fix the external pose integration (mostly the calibration part) from !362 (merged)

This MR contains various fixes about external sensors and calibration.

1. Add lever arm constraint in calibration estimation. This new constraint allows to fix the translation norm between the tracked frame (i.e. base -> often the Lidar frame) and the external sensor frame.
2. Add a window on trajectory during calibration upon two trajectories. This moving window allows to get rid of the drift in both trajectories and to only use local relative motion.
3. Refact the calibration interface in PV to be able to set parameters

It also contains style and refact minor changes.

Changes

1. Reset calibration in main Reset function (SLAM manager)
2. Update PoseManager in ExternalSensorManager file to add constraint options. Add a ceres constraint for the lever arm
3. Update SLAM interface to set the new parameters (trajectory window size and lerver arm distance)
4. Update PV interface with new parameters
5. Refact PV interface to better display advanced parameters
6. Add missing calibration for wheel encoder and gravity manager in PV wrapper
7. Add check when loading calibration from file
8. A calibrate button is added in PV interface to calibrate when the parameters for calibration matrix is set

Results

Reproduce

Data

Any velodyne data

Build

colcon build --symlink-install --base-paths src/slam/ros2_wrapping src/ouster --cmake-args -DCMAKE_BUILD_TYPE=Release

Parameters

To test:

lever_arm:= 1.

Run

• ros2 launch lidar_slam slam_velodyne.py
• ros2 bag play --clock -d 2 path/to/velodyne_data
• Run a first shot and save the trajectory as CSV file.
• Run a second time approximately the same number of frames
• Click on "Calibrate button"
• Choose the CSV file

Output

You should see a calibration matrix set to approx identity when lever_arm is kept to -1 and a calibration translation that tries to adapt when increasing the lever_arm.

/!\ Warning the saturation threshold is 0.5 by default so if the lever arm is too big the constraint won't be taken into account in the optimization

• Here is a resulting map
• Here is a resulting trajectory

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