Skip to content

Commit 4f4eebd

Browse files
YoshiRiYoshiRi
committed
feat(probabilistic_occupancy_grid_map): add downsample filter option to ogm creation (autowarefoundation#6865)
* feat: add downsample filter to ogm creation Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add downsample to multi_lidar ogm creation Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * chore: update package.xml and readme and param file Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * chore: update readme document Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * chore: fix downsample size Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * chore: fix cspell error Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add test_depend Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 71ddb01 commit 4f4eebd

5 files changed

+130
-13
lines changed

‎perception/probabilistic_occupancy_grid_map/README.md

+36
Original file line numberDiff line numberDiff line change
@@ -70,3 +70,39 @@ Additional argument is shown below:
7070
| `container_name` | `occupancy_grid_map_container` | |
7171
| `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud |
7272
| `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud |
73+
74+
### Downsample input pointcloud(Optional)
75+
76+
If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used. This feature is currently only for the pointcloud based occupancy grid map.
77+
78+
- pointcloud_based_occupancy_grid_map method
79+
80+
```yaml
81+
# downsampled raw and obstacle pointcloud
82+
/perception/occupancy_grid_map/obstacle/downsample/pointcloud
83+
/perception/occupancy_grid_map/raw/downsample/pointcloud
84+
```
85+
86+
- multi_lidar_pointcloud_based_point_cloud
87+
88+
```yaml
89+
# downsampled raw and obstacle pointcloud
90+
/perception/occupancy_grid_map/obstacle/downsample/pointcloud
91+
/perception/occupancy_grid_map/<sensor_name>/raw/downsample/pointcloud
92+
```
93+
94+
### Test
95+
96+
This package provides unit tests using `gtest`.
97+
You can run the test by the following command.
98+
99+
```bash
100+
colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+
101+
```
102+
103+
Test contains the following.
104+
105+
- Unit test for cost value conversion function
106+
- Unit test for utility functions
107+
- Unit test for occupancy grid map fusion functions
108+
- Input/Output test for pointcloud based occupancy grid map

‎perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,10 @@
88
min_height: -1.0
99
max_height: 2.0
1010

11+
# downsample input pointcloud
12+
downsample_input_pointcloud: true
13+
downsample_voxel_size: 0.25 # [m]
14+
1115
enable_single_frame_mode: false
1216
# use sensor pointcloud to filter obstacle pointcloud
1317
filter_obstacle_pointcloud_by_raw_pointcloud: false

‎perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py

+64-5
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,47 @@
2626
import yaml
2727

2828

29+
def get_downsample_filter_node(setting: dict) -> ComposableNode:
30+
plugin_str = setting["plugin"]
31+
voxel_size = setting["voxel_size"]
32+
node_name = setting["node_name"]
33+
return ComposableNode(
34+
package="pointcloud_preprocessor",
35+
plugin=plugin_str,
36+
name=node_name,
37+
remappings=[
38+
("input", setting["input_topic"]),
39+
("output", setting["output_topic"]),
40+
],
41+
parameters=[
42+
{
43+
"voxel_size_x": voxel_size,
44+
"voxel_size_y": voxel_size,
45+
"voxel_size_z": voxel_size,
46+
}
47+
],
48+
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
49+
)
50+
51+
52+
def get_downsample_preprocess_nodes(voxel_size: float) -> list:
53+
raw_settings = {
54+
"plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent",
55+
"node_name": "raw_pc_downsample_filter",
56+
"input_topic": LaunchConfiguration("input/raw_pointcloud"),
57+
"output_topic": "raw/downsample/pointcloud",
58+
"voxel_size": voxel_size,
59+
}
60+
obstacle_settings = {
61+
"plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent",
62+
"node_name": "obstacle_pc_downsample_filter",
63+
"input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
64+
"output_topic": "obstacle/downsample/pointcloud",
65+
"voxel_size": voxel_size,
66+
}
67+
return [get_downsample_filter_node(raw_settings), get_downsample_filter_node(obstacle_settings)]
68+
69+
2970
def launch_setup(context, *args, **kwargs):
3071
# load parameter files
3172
param_file = LaunchConfiguration("param_file").perform(context)
@@ -38,19 +79,37 @@ def launch_setup(context, *args, **kwargs):
3879
with open(updater_param_file, "r") as f:
3980
occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
4081

41-
composable_nodes = [
82+
# composable nodes
83+
composable_nodes = []
84+
85+
# add downsample process
86+
downsample_input_pointcloud: bool = pointcloud_based_occupancy_grid_map_node_params[
87+
"downsample_input_pointcloud"
88+
]
89+
if downsample_input_pointcloud:
90+
voxel_grid_size: float = pointcloud_based_occupancy_grid_map_node_params[
91+
"downsample_voxel_size"
92+
]
93+
downsample_preprocess_nodes = get_downsample_preprocess_nodes(voxel_grid_size)
94+
composable_nodes.extend(downsample_preprocess_nodes)
95+
96+
composable_nodes.append(
4297
ComposableNode(
4398
package="probabilistic_occupancy_grid_map",
4499
plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
45100
name="occupancy_grid_map_node",
46101
remappings=[
47102
(
48103
"~/input/obstacle_pointcloud",
49-
LaunchConfiguration("input/obstacle_pointcloud"),
104+
LaunchConfiguration("input/obstacle_pointcloud")
105+
if not downsample_input_pointcloud
106+
else "obstacle/downsample/pointcloud",
50107
),
51108
(
52109
"~/input/raw_pointcloud",
53-
LaunchConfiguration("input/raw_pointcloud"),
110+
LaunchConfiguration("input/raw_pointcloud")
111+
if not downsample_input_pointcloud
112+
else "raw/downsample/pointcloud",
54113
),
55114
("~/output/occupancy_grid_map", LaunchConfiguration("output")),
56115
],
@@ -60,8 +119,8 @@ def launch_setup(context, *args, **kwargs):
60119
{"updater_type": LaunchConfiguration("updater_type")},
61120
],
62121
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
63-
),
64-
]
122+
)
123+
)
65124

66125
occupancy_grid_map_container = ComposableNodeContainer(
67126
name=LaunchConfiguration("container_name"),

‎perception/probabilistic_occupancy_grid_map/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
<depend>tier4_autoware_utils</depend>
3535
<depend>visualization_msgs</depend>
3636

37+
<exec_depend>pointcloud_preprocessor</exec_depend>
3738
<exec_depend>pointcloud_to_laserscan</exec_depend>
3839

3940
<test_depend>ament_cmake_gtest</test_depend>
@@ -44,6 +45,7 @@
4445
<test_depend>launch_testing</test_depend>
4546
<test_depend>launch_testing_ament_cmake</test_depend>
4647
<test_depend>launch_testing_ros</test_depend>
48+
<test_depend>pointcloud_preprocessor</test_depend>
4749
<export>
4850
<build_type>ament_cmake</build_type>
4951
</export>

‎perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md

+24-8
Original file line numberDiff line numberDiff line change
@@ -77,18 +77,34 @@ $$
7777
| ----------------------------- | ------------------------- | ------------------ |
7878
| `~/output/occupancy_grid_map` | `nav_msgs::OccupancyGrid` | occupancy grid map |
7979

80+
### Related topics
81+
82+
If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used.
83+
84+
- pointcloud_based_occupancy_grid_map method
85+
86+
```yaml
87+
# downsampled raw and obstacle pointcloud
88+
/perception/occupancy_grid_map/obstacle/downsample/pointcloud
89+
/perception/occupancy_grid_map/raw/downsample/pointcloud
90+
```
91+
8092
## Parameters
8193

8294
### Node Parameters
8395

84-
| Name | Type | Description |
85-
| ------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- |
86-
| `map_frame` | string | map frame |
87-
| `base_link_frame` | string | base_link frame |
88-
| `use_height_filter` | bool | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. |
89-
| `map_length` | double | The length of the map. -100 if it is 50~50[m] |
90-
| `map_resolution` | double | The map cell resolution [m] |
91-
| `grid_map_type` | string | The type of grid map for estimating `UNKNOWN` region behind obstacle point clouds |
96+
| Name | Type | Description |
97+
| ----------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- |
98+
| `map_frame` | string | map frame |
99+
| `base_link_frame` | string | base_link frame |
100+
| `use_height_filter` | bool | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. |
101+
| `map_length` | double | The length of the map. -100 if it is 50~50[m] |
102+
| `map_resolution` | double | The map cell resolution [m] |
103+
| `grid_map_type` | string | The type of grid map for estimating `UNKNOWN` region behind obstacle point clouds |
104+
| `scan_origin` | string | The origin of the scan. It should be a sensor frame. |
105+
| `pub_debug_grid` | bool | Whether to publish debug grid maps |
106+
| `downsample_input_pointcloud` | bool | Whether to downsample the input pointclouds. The downsampled pointclouds are used for the ray tracing. |
107+
| `downsample_voxel_size` | double | The voxel size for the downsampled pointclouds. |
92108

93109
## Assumptions / Known limits
94110

0 commit comments

Comments
 (0)