feat(compare_map_segmentation): add dynamic map loading for voxel_bas…

…ed_compare_map_filter (autowarefoundation#3087) * feat: add interface to dynamic loader Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: refactoring Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: refactoring Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: refactoring Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: update readme Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: add default param and todo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: remove unnecessary neighbor voxels calculation Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add neighbor map_cell checking Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: neighbor map grid check Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
badai-nguyen · Apr 4, 2023 · ba31f6b · ba31f6b
1 parent 6f3b704
commit ba31f6b
Show file tree

Hide file tree

Showing 11 changed files with 1,001 additions and 248 deletions.
diff --git a/...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
@@ -38,6 +38,12 @@ def __init__(self, context):
         self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"]
         self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"]
         self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"]
+        self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"]
+        self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"]
+        self.map_update_distance_threshold = self.pointcloud_map_filter_param[
+            "map_update_distance_threshold"
+        ]
+        self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"]
 
     def create_pipeline(self):
         if self.use_down_sample_filter:
@@ -56,10 +62,17 @@ def create_normal_pipeline(self):
                     ("input", LaunchConfiguration("input_topic")),
                     ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
+                    ("map_loader_service", "/map/get_differential_pointcloud_map"),
+                    ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
                 ],
                 parameters=[
                     {
                         "distance_threshold": self.distance_threshold,
+                        "timer_interval_ms": self.timer_interval_ms,
+                        "use_dynamic_map_loading": self.use_dynamic_map_loading,
+                        "map_update_distance_threshold": self.map_update_distance_threshold,
+                        "map_loader_radius": self.map_loader_radius,
+                        "input_frame": "map",
                     }
                 ],
                 extra_arguments=[

diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt
@@ -27,6 +27,7 @@ add_library(compare_map_segmentation SHARED
   src/voxel_based_compare_map_filter_nodelet.cpp
   src/voxel_distance_based_compare_map_filter_nodelet.cpp
   src/compare_elevation_map_filter_node.cpp
+  src/voxel_grid_map_loader.cpp
 )
 
 target_link_libraries(compare_map_segmentation
@@ -44,6 +45,7 @@ ament_target_dependencies(compare_map_segmentation
   rclcpp_components
   sensor_msgs
   tier4_autoware_utils
+  autoware_map_msgs
 )
 
 if(OPENMP_FOUND)

diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md
@@ -2,7 +2,7 @@
 
 ## Purpose
 
-The `compare_map_segmentation` is a node that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map).
+The `compare_map_segmentation` is a node that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map or split map pointcloud from map_loader interface).
 
 ## Inner-workings / Algorithms
 
@@ -24,7 +24,9 @@ WIP
 
 ### Voxel Based Compare Map Filter
 
-WIP
+The filter loads the map pointcloud (static loading whole map at once at beginning or dynamic loading during vehicle moving) and utilizes VoxelGrid to downsample map pointcloud.
+
+For each point of input pointcloud, the filter use `getCentroidIndexAt` combine with `getGridCoordinates` function from VoxelGrid class to check if the downsampled map point existing surrounding input points. Remove the input point which has downsampled map point in voxels containing or being close to the point.
 
 ### Voxel Distance based Compare Map Filter
 
@@ -47,30 +49,39 @@ WIP
 | ----------------- | ------------------------------- | --------------- |
 | `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
 
+#### Parameters
+
+| Name                 | Type   | Description                                                                     | Default value |
+| :------------------- | :----- | :------------------------------------------------------------------------------ | :------------ |
+| `map_layer_name`     | string | elevation map layer name                                                        | elevation     |
+| `map_frame`          | float  | frame_id of the map that is temporarily used before elevation_map is subscribed | map           |
+| `height_diff_thresh` | float  | Remove points whose height difference is below this value [m]                   | 0.15          |
+
 ### Other Filters
 
 #### Input
 
-| Name             | Type                            | Description      |
-| ---------------- | ------------------------------- | ---------------- |
-| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points |
-| `~/input/map`    | `grid_map::msg::GridMap`        | map              |
+| Name                     | Type                                            | Description                                            |
+| ------------------------ | ----------------------------------------------- | ------------------------------------------------------ |
+| `~/input/points`         | `sensor_msgs::msg::PointCloud2`                 | reference points                                       |
+| `~/input/map`            | `sensor_msgs::msg::PointCloud2`                 | map (in case static map loading)                       |
+| `~/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading) |
 
 #### Output
 
 | Name              | Type                            | Description     |
 | ----------------- | ------------------------------- | --------------- |
 | `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
 
-## Parameters
-
-### Core Parameters
+#### Parameters
 
-| Name                 | Type   | Description                                                                     | Default value |
-| :------------------- | :----- | :------------------------------------------------------------------------------ | :------------ |
-| `map_layer_name`     | string | elevation map layer name                                                        | elevation     |
-| `map_frame`          | float  | frame_id of the map that is temporarily used before elevation_map is subscribed | map           |
-| `height_diff_thresh` | float  | Remove points whose height difference is below this value [m]                   | 0.15          |
+| Name                            | Type  | Description                                                                                  | Default value |
+| :------------------------------ | :---- | :------------------------------------------------------------------------------------------- | :------------ |
+| `use_dynamic_map_loading`       | bool  | map loading mode selection, `true` for dynamic map loading, `false` for static map loading   | true          |
+| `distance_threshold`            | float | VoxelGrid's leaf_size also the threshold to check distance between input point and map point | 0.5           |
+| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary [m]                      | 10.0          |
+| `map_loader_radius`             | float | Radius of map need to be loaded (in dynamic map loading) [m]                                 | 150.0         |
+| `timer_interval_ms`             | int   | time interval of timer to check if update the map is necessary (in dynamic map loading) [ms] | 100           |
 
 ## Assumptions / Known limits