Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat: move elevation map loader (#740) #136

Merged
merged 9 commits into from
Dec 8, 2021
Merged

feat: move elevation map loader (#740) #136

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
dd09dff
feat: Move elevation map loader (#740)
Dec 6, 2021
1eca89b
Update perception/elevation_map_loader/README.md
wep21 Dec 7, 2021
4a5794e
Update perception/elevation_map_loader/README.md
wep21 Dec 7, 2021
1546c0a
Update perception/elevation_map_loader/README.md
wep21 Dec 7, 2021
8255d60
Merge branch 'tier4/proposal' into 740-AAP
tkimura4 Dec 7, 2021
4d76a2e
Merge branch 'tier4/proposal' into 740-AAP
tkimura4 Dec 7, 2021
8e13cba
Merge branch 'tier4/proposal' into 740-AAP
1222-takeshi Dec 8, 2021
0e4bcc9
Merge branch 'tier4/proposal' into 740-AAP
1222-takeshi Dec 8, 2021
3627a83
Merge branch 'tier4/proposal' into 740-AAP
tkimura4 Dec 8, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 0 additions & 12 deletions map/map_loader/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,16 +26,6 @@ rclcpp_components_register_node(pointcloud_map_loader_node
EXECUTABLE pointcloud_map_loader
)

ament_auto_add_library(elevation_map_loader_node SHARED
src/elevation_map_loader/elevation_map_loader_node.cpp
)
target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES})

rclcpp_components_register_node(elevation_map_loader_node
PLUGIN "ElevationMapLoaderNode"
EXECUTABLE elevation_map_loader
)

ament_auto_add_library(lanelet2_map_loader_node SHARED
src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
)
Expand Down Expand Up @@ -66,6 +56,4 @@ endif()

ament_auto_package(INSTALL_TO_SHARE
launch
config
data
)
111 changes: 0 additions & 111 deletions map/map_loader/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,114 +52,3 @@ lanelet2_map_visualization visualizes autoware_lanelet2_msgs/MapBin messages int
### Published Topics

- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz

---

## elevation_map_loader

### Feature

Generate elevation_map from subscribed pointcloud_map and vector_map and publish it.
Save the generated elevation_map locally and load it from next time.

The elevation value of each cell is the average value of z of the points of the lowest cluster.
Cells with No elevation value can be inpainted using the values of neighboring cells.

<p align="center">
<img src="./media/elevation_map.png" width="1500">
</p>

### How to run

`ros2 run map_loader elevation_map_loader --ros-args -p param_file_path:=path/to/elevation_map_parameters.yaml -p elevation_map_directory:=path/to/elevation_map_directory -p pointcloud_map_path:=path/to/pointcloud.pcd`

### Subscribed Topics

- input/pointcloud_map (sensor_msgs:PointCloud2) : PointCloud Map
- input/vector_map (autoware_lanelet2_msgs/MapBin) : binary data of Lanelet2 Map

### Published Topics

- output/elevation_map (grid_map_msgs/GridMap) : Elevation Map
- output/elevation_map_cloud (sensor_msgs:PointCloud2) : Pointcloud generated from the value of Elevation Map

### Parameter description

#### ROS parameters

| Name | Type | Description | Default value |
| :-------------------------------- | :---------- | :--------------------------------------------------------------------------------------------------------- | :------------ |
| map_layer_name | std::string | elevation_map layer name | elevation |
| param_file_path | std::string | GridMap parameters config | path_default |
| elevation_map_file_path | std::string | elevation_map file (bag2) | path_default |
| map_frame | std::string | map_frame when loading elevation_map file | map |
| use_inpaint | bool | Whether to inpaint empty cells | true |
| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 |
| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false |
| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false |
| lane_margin | float | Value of how much to expand the range of vector_map [m] | 0.5 |
| lane_height_diff_thresh | float | Only point clouds in the height range of this value from vector_map are used to generate elevation_map [m] | 1.0 |
| lane_filter_voxel_size_x | float | Voxel size x for calculating point clouds in vector_map [m] | 0.04 |
| lane_filter_voxel_size_y | float | Voxel size y for calculating point clouds in vector_map [m] | 0.04 |
| lane_filter_voxel_size_z | float | Voxel size z for calculating point clouds in vector_map [m] | 0.04 |

#### GridMap parameters

The parameters are described on `config/elevation_map_parameters.yaml`.

##### General parameters

| Name | Type | Description | Default value |
| :--------------------------------------------- | :--- | :----------------------------------------------------------------------------------------------------------- | :------------ |
| pcl_grid_map_extraction/num_processing_threads | int | Number of threads for processing grid map cells. Filtering of the raw input point cloud is not parallelized. | 12 |

##### Grid map parameters

See: <https://github.com/ANYbotics/grid_map/tree/ros2/grid_map_pcl>

Resulting grid map parameters.

| Name | Type | Description | Default value |
| :------------------------------------------------------- | :---- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| pcl_grid_map_extraction/grid_map/min_num_points_per_cell | int | Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN. | 3 |
| pcl_grid_map_extraction/grid_map/resolution | float | Resolution of the grid map. Width and length are computed automatically. | 0.3 |

#### Point Cloud Pre-processing Parameters

##### Rigid body transform parameters

Rigid body transform that is applied to the point cloud before computing elevation.

| Name | Type | Description | Default value |
| :-------------------------------------------------- | :---- | :---------------------------------------------------------------------------------------------------------------------- | :------------ |
| pcl_grid_map_extraction/cloud_transform/translation | float | Translation (xyz) that is applied to the input point cloud before computing elevation. | 0.0 |
| pcl_grid_map_extraction/cloud_transform/rotation | float | Rotation (intrinsic rotation, convention X-Y'-Z'') that is applied to the input point cloud before computing elevation. | 0.0 |

##### Cluster extraction parameters

Cluster extraction is based on pcl algorithms. See <https://pointclouds.org/documentation/tutorials/cluster_extraction.html> for more details.

| Name | Type | Description | Default value |
| :----------------------------------------------------------- | :---- | :------------------------------------------------------------------------------------- | :------------ |
| pcl_grid_map_extraction/cluster_extraction/cluster_tolerance | float | Distance between points below which they will still be considered part of one cluster. | 0.2 |
| pcl_grid_map_extraction/cluster_extraction/min_num_points | int | Min number of points that a cluster needs to have (otherwise it will be discarded). | 3 |
| pcl_grid_map_extraction/cluster_extraction/max_num_points | int | Max number of points that a cluster can have (otherwise it will be discarded). | 1000000 |

##### Outlier removal parameters

See <https://pointclouds.org/documentation/tutorials/statistical_outlier.html> for more explanation on outlier removal.

| Name | Type | Description | Default value |
| :--------------------------------------------------------- | :---- | :----------------------------------------------------------------------------- | :------------ |
| pcl_grid_map_extraction/outlier_removal/is_remove_outliers | float | Whether to perform statistical outlier removal. | false |
| pcl_grid_map_extraction/outlier_removal/mean_K | float | Number of neighbours to analyze for estimating statistics of a point. | 10 |
| pcl_grid_map_extraction/outlier_removal/stddev_threshold | float | Number of standard deviations under which points are considered to be inliers. | 1.0 |

##### Subsampling parameters

See <https://pointclouds.org/documentation/tutorials/voxel_grid.html> for more explanation on point cloud downsampling.

| Name | Type | Description | Default value |
| :------------------------------------------------------- | :---- | :-------------------------------------- | :------------ |
| pcl_grid_map_extraction/downsampling/is_downsample_cloud | bool | Whether to perform downsampling or not. | false |
| pcl_grid_map_extraction/downsampling/voxel_size | float | Voxel sizes (xyz) in meters. | 0.02 |
5 changes: 0 additions & 5 deletions map/map_loader/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,8 @@
<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_utils</depend>
<depend>geometry_msgs</depend>
<depend>grid_map_cv</depend>
<depend>grid_map_pcl</depend>
<depend>grid_map_ros</depend>
<depend>hash_library_vendor</depend>
<depend>lanelet2_extension</depend>
<depend>libpcl-all-dev</depend>
<depend>nlohmann-json-dev</depend>
<depend>pcl_conversions</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand Down
38 changes: 38 additions & 0 deletions perception/elevation_map_loader/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 3.5)
project(elevation_map_loader)

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
endif()

find_package(PCL REQUIRED COMPONENTS io)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_library(elevation_map_loader_node SHARED
src/elevation_map_loader_node.cpp
)
target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES})

rclcpp_components_register_node(elevation_map_loader_node
PLUGIN "ElevationMapLoaderNode"
EXECUTABLE elevation_map_loader
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(INSTALL_TO_SHARE
launch
config
data
)
Loading