[ROS2][feat] Extract obstacles in aggregated frame

Goal

Detect obstacles relatively to a reference map.

Changes

slam lib

• Increase label size in LidarSlam point to store cluster info (>255)
• Add function LabelNewPoints in Rolling grid to labela pointcloud using the rolling grid labels
• In the add function, the label of the input point is forced as the voxel grid label (if voxel not fixed)
• Add enum LidarPointLabel
• Fix and uniformize Rolling grid

AggregationNode

New functions

• LoadInitMap : Load an initial map in the aggregation node. All points are considered as fixed.
• LabelObstacle : Take a labeled pointcloud with potential obstacles as input and label obstacle clusters in it. Steps are:
  • Project current points into the occupancy grid
  • Update time of each occupancy grid pixel
  • Label pixels that can be clusterized (all pixels that have been seen at least once)
  • Grow existing regions
  • Get potential seeds in remaining not labeled pixels
  • Grow new regions
  • Clear the occupancy grid
  • Remove old clusters (using the decay time parameter to remove clusters which correspond to moving objects)

Results

Reproduce

Build

colcon build --base-paths slam/ros2_wrapping --cmake-args -DCMAKE_BUILD_TYPE=Release

Test

This process is divided into 2 phases :

1. Create the reference map on an empty pitch
2. Use the reference map to detect obstacles in the pitch.

Data

To create the reference :

pitch_1_record_2

To test the obstacles detection :

zig-zag-between-rigs

Parameters

Parameter files to create the reference map

Parameter files to detect obstacles -> NOTE : paths to files need to be updated to your specific locations (see params init_map_path and initial_maps).

Run

ros2 launch lidar_slam slam_velodyne.py aggregate:=true

ros2 bag play --clock --start-offset 35 -d 2 'data/path/to/bag/zig-zag-between-rigs'

Output

• Reference maps output from phase 1 (it can be directly used in the detection phase)
• Demo video

SLAM map with obstacle markers

Obstacle makers

Occupancy grid with clusters

Note :

• min_points_per_voxel parameter have a strong impact on results to remove moving objects.
• moving people are still extracted (see traces in the obstacle map).
• The decay time allows to forget obstacles and so moving objects in time.

Performances

SLAM

• 26ms
• Keypoints extraction is abnormally long (11ms). To be inspected.

Obstacles extraction

• ~8ms
• Time could be reduced further

NOTE: Rviz struggles in displaying the markers, especially when a dimension is greater than the others. So one may experience tilting boxes.

Checklist

• Camel case everywhere except for ROS variables/parameters
• Lower case for local variables and lambda functions
• Upper case for 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)
• Check your spaces
  • between if, while, for and parenthesis
  • between operators and variables: e.g. a + b
  • 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
• 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

TODO

• Improve performances
• Write documentation
• Check tilting boxes bug in rviz