[ROS2][feat][fix] Add interface for saturation with ext poses

Goal

The goal is to add a saturation interface to reject outlier external poses during optimization.

Changes

Feat

• ROS interface is updated (param files + LidarSlamNode)

Fixes

• Fix the link break between IMU and external poses when resetting external pose manager. The specific parameters of the external pose sensor were forgotten during link break.
• Allow the lever arm to be 0. This was not handled, the constraint was not taken into account.
• Fix frame ID in PV. This was avoiding the LiDAR calibration to be properly set.

fact

• Refact the constructors of the external sensors to only keep the common parameters. Specific sensors' parameters must be set with relative setters.

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:= 10.

lever_arm:= 0.

saturation_distance:= 10000.

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

• With lever_arm to 10000, you should see that the resulting calibration translation is really increased.
• With lever_arm to 0, you should see that the calibration is (or really close to) identity.