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,57 @@ def launch_setup(context, *args, **kwargs):
         output="screen",
     )
 
-    # load compare map for dynamic obstacle stop module
+    # This condition determine whether the points filter below are launched
+    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_no_detection_area_filter = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare("tier4_planning_launch"),
+                "/launch/scenario_planning/lane_driving/behavior_planning/no_detection_area_filter.launch.py",
+            ]
         ),
+        launch_arguments={
+            "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+            "container_name": LaunchConfiguration("container_name"),
+            "use_multithread": "true",
+        }.items(),
+        # launch no detection area 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_no_detection_area_filter,
         ]
     )
 

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", "no_detection_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/no_detection_area_filter.launch.py

@@ -0,0 +1,83 @@
+# 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::NoDetectionAreaFilterComponent",
+            name="no_detection_area_filter",
+            remappings=[
+                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("input/vector_map", "/map/vector_map"),
+                ("output", "no_detection_area_filtered/pointcloud"),
+            ],
+            # this node has QoS of transient local
+            extra_arguments=[{"use_intra_process_comms": False}],
+        )
+    ]
+
+    no_detection_area_filter_container = ComposableNodeContainer(
+        name="no_detection_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", "no_detection_area_filter_container"),
+            set_container_executable,
+            set_container_mt_executable,
+            no_detection_area_filter_container,
+            load_composable_nodes,
+        ]
+    )

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -114,10 +114,13 @@ 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_detection_area =
+    lanelet::utils::query::getAllNoDetectionArea(viz_lanelet_map);
 
   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_detection_area;
   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 +136,7 @@ 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_detection_area, 0.37, 0.37, 0.27, 0.5);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -196,6 +200,9 @@ void Lanelet2MapVisualizationNode::onMapBin(
   insertMarkerArray(
     &map_marker_array,
     lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road));
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::noDetectionAreaAsMarkerArray(no_detection_area, cl_no_detection_area));
 
   pub_marker_->publish(map_marker_array);
 }

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -40,6 +40,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/no_detection_area_filter/no_detection_area_filter.cpp
 )
 
 target_link_libraries(pointcloud_preprocessor_filter
@@ -60,13 +61,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 +113,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 +141,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)
 
+# ========== No Detection Area Filter ===========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::NoDetectionAreaFilterComponent"
+  EXECUTABLE no_detection_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)        |
+| no_detection_area_filter | remove points inside of lanelet maps tagged as no detection area                   | [link](docs/no-detection-area-filter.md) |
 
 ## Inputs / Outputs
 

sensing/pointcloud_preprocessor/docs/no-detection-area-filter.md

@@ -0,0 +1,36 @@
+# no_detection_area_filter
+
+## Purpose
+
+The `no_detection_area_filter` is a node that removes points inside the area tagged as `no_detection_area` in lanelet map.
+
+## Inner-workings / Algorithms
+
+- Extract the `no_detection_area` lanelets that intersects with the bounding box of input points
+- Create the 2D polygon from the extracted lanelet polygons
+- Remove input points inside the polygon by `boost::geometry::within()`
+
+![no_detection_area_figure](./image/no_detection_area_filter-overview.svg)
+
+## Inputs / Outputs
+
+This implementation inherit `pointcloud_preprocessor::Filter` class, 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` | lanelet2 map used for filtering points |
+
+### Output
+
+| Name       | Type                            | Description     |
+| ---------- | ------------------------------- | --------------- |
+| `~/output` | `sensor_msgs::msg::PointCloud2` | filtered points |
+
+## Parameters
+
+### Core Parameters
+
+## Assumptions / Known limits

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/no_detection_area_filter/no_detection_area_filter.hpp

@@ -0,0 +1,70 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// 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.
+
+#ifndef POINTCLOUD_PREPROCESSOR__NO_DETECTION_AREA_FILTER__NO_DETECTION_AREA_FILTER_HPP_
+#define POINTCLOUD_PREPROCESSOR__NO_DETECTION_AREA_FILTER__NO_DETECTION_AREA_FILTER_HPP_
+
+#include "pointcloud_preprocessor/filter.hpp"
+
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_debug_msgs/msg/float32_stamped.hpp>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+#endif
+
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::Point2d;
+
+namespace pointcloud_preprocessor
+{
+class NoDetectionAreaFilterComponent : public pointcloud_preprocessor::Filter
+{
+private:
+  void filter(
+    const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
+  lanelet::ConstPolygons3d no_detection_area_lanelets_;
+
+  void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+  explicit NoDetectionAreaFilterComponent(const rclcpp::NodeOptions & options);
+};
+
+}  // namespace pointcloud_preprocessor
+
+#endif  // POINTCLOUD_PREPROCESSOR__NO_DETECTION_AREA_FILTER__NO_DETECTION_AREA_FILTER_HPP_