[ROS2][feat] Extract obstacle in aggregated frame

Goal

Extract obstacle points with respect to a reference map in the aggregation node of ros_wrapping.

Changes

In slam lib:

• A function called SampleNonFixedPoint is added to RollingGrid in slam. It takes a pointcloud as input and label points in the pointcloud. It samples non-fixed points in a pointcloud with respect to a fixed current map. Sampled points are labeled as 0, others as 1. The sampling mode is to take the first point.
• Add enum LidarPointLabel

New functions in AggregationNode in ros2_wrapping:

• LoadReferenceMap: all points are considered as fixed in the reference map
• GrowRegion: Segment region in the image of occupancy grid for a seed pixel
• ExtractObstacle: it takes a labeled pointcloud as input and obstacle points as output. Please see the following figure

Results

Reproduce

Data

I have used those data to test: bag/zig-zag-between-rigs

Build

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

Parameters

Here are the parameters files that I used:

slam_config_outdoor.yaml

aggregation_config.yaml.

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

• Here is a demo of result

• Here is the occupancy grid:

• Here is the obstacle points

• Here is the bounding boxes of clusters

Performances (optional)

• The average time for extraction obstacle is 0.038s

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

What remains to solve

• In GrowRegion function: a merge region functionality could be added here. For example a region 3 could reach region 5 with new frames arrive. But since at the beginning, they are not reached so they are not considered as the same region.
• Find a better way to sample and label obstacles points. For now, only the first points that visited a non-fixed voxel of DenseMap are labeled as obstacle.
• The runtime is 38ms. Is it real time enough?