[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:
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?
Edited by Tong Fu