feat: add vector map inside area filter

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -384,39 +384,58 @@ def launch_setup(context, *args, **kwargs):
         output="screen",
     )
 
-    # load compare map for dynamic obstacle stop module
+    # This condition is true if run_out module is enabled and its detection method is Points
+    launch_run_out_with_points_method = PythonExpression(
+        [
+            LaunchConfiguration(
+                "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
+            ),
+            " and ",
+            "'",
+            run_out_param["run_out"]["detection_method"],
+            "' == 'Points'",
+        ]
+    )
+
+    # load compare map for run_out module
     load_compare_map = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
                 FindPackageShare("tier4_planning_launch"),
                 "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py",
-            ]
+            ],
         ),
         launch_arguments={
             "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
             "container_name": LaunchConfiguration("container_name"),
             "use_multithread": "true",
         }.items(),
         # launch compare map only when run_out module is enabled and detection method is Points
-        condition=IfCondition(
-            PythonExpression(
-                [
-                    LaunchConfiguration(
-                        "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
-                    ),
-                    " and ",
-                    "'",
-                    run_out_param["run_out"]["detection_method"],
-                    "' == 'Points'",
-                ]
-            )
+        condition=IfCondition(launch_run_out_with_points_method),
+    )
+
+    load_vector_map_inside_area_filter = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare("tier4_planning_launch"),
+                "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py",
+            ]
         ),
+        launch_arguments={
+            "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+            "container_name": LaunchConfiguration("container_name"),
+            "use_multithread": "true",
+            "polygon_type": "no_obstacle_segmentation_area_for_run_out",
+        }.items(),
+        # launch vector map filter only when run_out module is enabled and detection method is Points
+        condition=IfCondition(launch_run_out_with_points_method),
     )
 
     group = GroupAction(
         [
             container,
             load_compare_map,
+            load_vector_map_inside_area_filter,
         ]
     )
 
@@ -451,7 +470,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_intra_process", "false", "use ROS2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")
 
-    # for compare map
+    # for points filter of run out module
     add_launch_arg("use_pointcloud_container", "true")
     add_launch_arg("container_name", "pointcloud_container")
 

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py

@@ -45,7 +45,7 @@ def add_launch_arg(name: str, default_value=None):
             plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent",
             name="voxel_distance_based_compare_map_filter_node",
             remappings=[
-                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("input", "vector_map_inside_area_filtered/pointcloud"),
                 ("map", "/map/pointcloud_map"),
                 ("output", "compare_map_filtered/pointcloud"),
             ],
@@ -61,7 +61,7 @@ def add_launch_arg(name: str, default_value=None):
     ]
 
     compare_map_container = ComposableNodeContainer(
-        name=LaunchConfiguration("container_name"),
+        name="compare_map_container",
         namespace="",
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py

@@ -0,0 +1,88 @@
+# Copyright 2022 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    def add_launch_arg(name: str, default_value=None):
+        return DeclareLaunchArgument(name, default_value=default_value)
+
+    set_container_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container",
+        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    set_container_mt_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container_mt",
+        condition=IfCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    composable_nodes = [
+        ComposableNode(
+            package="pointcloud_preprocessor",
+            plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent",
+            name="vector_map_inside_area_filter_node",
+            remappings=[
+                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("input/vector_map", "/map/vector_map"),
+                ("output", "vector_map_inside_area_filtered/pointcloud"),
+            ],
+            parameters=[
+                {
+                    "polygon_type": LaunchConfiguration("polygon_type"),
+                }
+            ],
+            # this node has QoS of transient local
+            extra_arguments=[{"use_intra_process_comms": False}],
+        ),
+    ]
+
+    vector_map_area_filter_container = ComposableNodeContainer(
+        name="vector_map_area_filter_container",
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=composable_nodes,
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+        output="screen",
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        composable_node_descriptions=composable_nodes,
+        target_container=LaunchConfiguration("container_name"),
+        condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+
+    return LaunchDescription(
+        [
+            add_launch_arg("use_multithread", "true"),
+            add_launch_arg("use_pointcloud_container", "true"),
+            add_launch_arg("container_name", "vector_map_area_filter_container"),
+            set_container_executable,
+            set_container_mt_executable,
+            vector_map_area_filter_container,
+            load_composable_nodes,
+        ]
+    )

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -114,10 +114,16 @@ void Lanelet2MapVisualizationNode::onMapBin(
   lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map);
   lanelet::ConstPolygons3d obstacle_polygons =
     lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map);
+  lanelet::ConstPolygons3d no_obstacle_segmentation_area =
+    lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area");
+  lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out =
+    lanelet::utils::query::getAllPolygonsByType(
+      viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out");
 
   std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
     cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
-    cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas;
+    cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas,
+    cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -133,6 +139,8 @@ void Lanelet2MapVisualizationNode::onMapBin(
   setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3);
   setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6);
   setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999);
+  setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5);
+  setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -196,6 +204,13 @@ void Lanelet2MapVisualizationNode::onMapBin(
   insertMarkerArray(
     &map_marker_array,
     lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road));
+  insertMarkerArray(
+    &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray(
+                         no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area));
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray(
+      no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out));
 
   pub_marker_->publish(map_marker_array);
 }

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -23,6 +23,7 @@ include_directories(
 
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/filter.cpp
+  src/utility/utilities.cpp
   src/concatenate_data/concatenate_data_nodelet.cpp
   src/crop_box_filter/crop_box_filter_nodelet.cpp
   src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
@@ -40,6 +41,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/distortion_corrector/distortion_corrector.cpp
   src/blockage_diag/blockage_diag_nodelet.cpp
   src/polygon_remover/polygon_remover.cpp
+  src/vector_map_filter/vector_map_inside_area_filter.cpp
 )
 
 target_link_libraries(pointcloud_preprocessor_filter
@@ -60,13 +62,11 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
   EXECUTABLE concatenate_data_node)
 
-
 # ========== CropBox ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::CropBoxFilterComponent"
   EXECUTABLE crop_box_filter_node)
 
-
 # ========== Down Sampler Filter ==========
 # -- Voxel Grid Downsample Filter --
 rclcpp_components_register_node(pointcloud_preprocessor_filter
@@ -114,19 +114,16 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::PassThroughFilterUInt16Component"
   EXECUTABLE passthrough_filter_uint16_node)
 
-
 # ========== Pointcloud Accumulator Filter ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::PointcloudAccumulatorComponent"
   EXECUTABLE pointcloud_accumulator_node)
 
-
 # ========== Vector Map Filter ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::Lanelet2MapFilterComponent"
   EXECUTABLE vector_map_filter_node)
 
-
 # ========== Distortion Corrector ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::DistortionCorrectorComponent"
@@ -145,6 +142,11 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
 set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE)
 target_link_libraries(polygon_remover_node gmp CGAL CGAL::CGAL CGAL::CGAL_Core)
 
+# ========== Vector Map Inside Area Filter ===========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::VectorMapInsideAreaFilterComponent"
+  EXECUTABLE vector_map_inside_area_filter_node)
+
 ament_auto_package(INSTALL_TO_SHARE
   launch
 )

sensing/pointcloud_preprocessor/README.md

@@ -14,16 +14,17 @@ The `pointcloud_preprocessor` is a package that includes the following filters:
 
 Detail description of each filter's algorithm is in the following links.
 
-| Filter Name            | Description                                                                        | Detail                                 |
-| ---------------------- | ---------------------------------------------------------------------------------- | -------------------------------------- |
-| concatenate_data       | subscribe multiple pointclouds and concatenate them into a pointcloud              | [link](docs/concatenate-data.md)       |
-| crop_box_filter        | remove points within a given box                                                   | [link](docs/crop-box-filter.md)        |
-| distortion_corrector   | compensate pointcloud distortion caused by ego vehicle's movement during 1 scan    | [link](docs/distortion-corrector.md)   |
-| downsample_filter      | downsampling input pointcloud                                                      | [link](docs/downsample-filter.md)      |
-| outlier_filter         | remove points caused by hardware problems, rain drops and small insects as a noise | [link](docs/outlier-filter.md)         |
-| passthrough_filter     | remove points on the outside of a range in given field (e.g. x, y, z, intensity)   | [link](docs/passthrough-filter.md)     |
-| pointcloud_accumulator | accumulate pointclouds for a given amount of time                                  | [link](docs/pointcloud-accumulator.md) |
-| vector_map_filter      | remove points on the outside of lane by using vector map                           | [link](docs/vector-map-filter.md)      |
+| Filter Name                   | Description                                                                        | Detail                                        |
+| ----------------------------- | ---------------------------------------------------------------------------------- | --------------------------------------------- |
+| concatenate_data              | subscribe multiple pointclouds and concatenate them into a pointcloud              | [link](docs/concatenate-data.md)              |
+| crop_box_filter               | remove points within a given box                                                   | [link](docs/crop-box-filter.md)               |
+| distortion_corrector          | compensate pointcloud distortion caused by ego vehicle's movement during 1 scan    | [link](docs/distortion-corrector.md)          |
+| downsample_filter             | downsampling input pointcloud                                                      | [link](docs/downsample-filter.md)             |
+| outlier_filter                | remove points caused by hardware problems, rain drops and small insects as a noise | [link](docs/outlier-filter.md)                |
+| passthrough_filter            | remove points on the outside of a range in given field (e.g. x, y, z, intensity)   | [link](docs/passthrough-filter.md)            |
+| pointcloud_accumulator        | accumulate pointclouds for a given amount of time                                  | [link](docs/pointcloud-accumulator.md)        |
+| vector_map_filter             | remove points on the outside of lane by using vector map                           | [link](docs/vector-map-filter.md)             |
+| vector_map_inside_area_filter | remove points inside of vector map area that has given type by parameter           | [link](docs/vector-map-inside-area-filter.md) |
 
 ## Inputs / Outputs
 

sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md

@@ -0,0 +1,39 @@
+# vector_map_inside_area_filter
+
+## Purpose
+
+The `vector_map_inside_area_filter` is a node that removes points inside the vector map area that has given type by parameter.
+
+## Inner-workings / Algorithms
+
+- Get the vector map area that has given type by parameter of `polygon_type`
+- Extract the vector map area that intersects with the bounding box of input points to reduce the calculation cost
+- Create the 2D polygon from the extracted vector map area
+- Remove input points inside the polygon
+
+![vector_map_inside_area_filter_figure](./image/vector_map_inside_area_filter_overview.svg)
+
+## Inputs / Outputs
+
+This implementation inherits `pointcloud_preprocessor::Filter` class, so please see also [README](../README.md).
+
+### Input
+
+| Name                 | Type                                         | Description                          |
+| -------------------- | -------------------------------------------- | ------------------------------------ |
+| `~/input`            | `sensor_msgs::msg::PointCloud2`              | input points                         |
+| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map used for filtering points |
+
+### Output
+
+| Name       | Type                            | Description     |
+| ---------- | ------------------------------- | --------------- |
+| `~/output` | `sensor_msgs::msg::PointCloud2` | filtered points |
+
+### Core Parameters
+
+| Name           | Type   | Description                 |
+| -------------- | ------ | --------------------------- |
+| `polygon_type` | string | polygon type to be filtered |
+
+## Assumptions / Known limits