[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
Edited by Julia Sanchez