[ROS2][feat] Allow to remove map points around the trajectory

Goal

Bringing ROS1 changes from !350 (merged) and !358 (merged) to ROS2

In the aggregated map (from aggregationNode), remove points that lay at the same place as was the Lidar sensor before. They should belong to a moving object.

Changes

Rolling Grid

• Add EmptyAroundPoint function in RollingGrid to remove all points around an input point in the current map
• A tool function Erase is added in RollingGrid too, to factorize between ClearPoints and EmptyAroundPoint
• The construction of the submap and the kdtree are split into two functions. This can be reused in the future if the search feature is not needed.

Aggregation Node

• Allow to remove points around pose in aggregation
• Add a distance parameter
• Add the feature to remove points in pose callback
• Remove the slice condition for the pose subcriber. Now, the aggregation node always subscribes to the SLAM pose

LidarSlamNode

Minor change to remove unused cout output when verbosity is < 3

Results

Reproduce

Data

fast-filtered.bag

Build

catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release

Parameters

 wheel_encoder:
    enable: true            # [bool] To receive and use a wheel encoder (distance measurement)
    relative: true          # [bool] To set the constraint from reference or from previous frame
    weight: 10000.

  ke:
    ...
    input_sampling_ratio: 0.25          # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
    

Run

ros2 launch lidar_slam slam_ouster.launch outdoor:=false aggregate:=true

ros2 bag play --clock -d 2 /home/julia/Desktop/Projects/d/da/DataDuctus/data/tunnel/fast-filtered --remap /wire_converted:=wheel_odom /lidar_points:=/ouster/points

Output

The aggregated map before and after the new process is stored here.

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

What remains to solve

• Do the same for the SLAM maps