feat: add down sample filter before detection module (autowarefoundat…

…ion#961)

* feat: add down sample filter before detection module

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix format

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* change comment

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* add output topic

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* Update launch/tier4_perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Update launch/tier4_perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* ci(pre-commit): autofix

* fix pre-commit

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

launch/tier4_perception_launch/config/object_recognition/detection/detection_preprocess.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    use_down_sample_filter: False
+    down_sample_voxel_size: 0.1
+    distance_threshold: 0.5

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -21,6 +21,8 @@
   <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
+  <arg name="container_name" default=""/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -37,11 +39,13 @@
 
   <!-- Pointcloud map filter -->
   <group>
-    <include file="$(find-pkg-share compare_map_segmentation)/launch/voxel_based_compare_map_filter.launch.xml" if="$(var use_pointcloud_map)">
-      <arg name="input" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="input_map" value="/map/pointcloud_map"/>
-      <arg name="output" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="distance_threshold" value="0.5"/>
+    <include file="$(find-pkg-share perception_launch)/launch/object_recognition/detection/detection_preprocess.launch.py" if="$(var use_pointcloud_map)">
+      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
+      <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
+      <arg name="use_intra_process" value="true"/>
+      <arg name="use_multithread" value="true"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="container_name" value="$(var pointcloud_container_name)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py

@@ -0,0 +1,168 @@
+# 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.
+import os
+
+from ament_index_python.packages import get_package_share_directory
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+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
+import yaml
+
+
+class DetectionPreProcessPipeline:
+    def __init__(self, context):
+        detection_preprocess_param_path = os.path.join(
+            get_package_share_directory("perception_launch"),
+            "config/object_recognition/detection/detection_preprocess.param.yaml",
+        )
+        with open(detection_preprocess_param_path, "r") as f:
+            self.detection_preprocess_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+        self.use_down_sample_filter = self.detection_preprocess_param["use_down_sample_filter"]
+        self.voxel_size = self.detection_preprocess_param["down_sample_voxel_size"]
+        self.distance_threshold = self.detection_preprocess_param["distance_threshold"]
+
+    def create_pipeline(self):
+        if self.use_down_sample_filter:
+            return self.create_down_sample_pipeline()
+        else:
+            return self.create_normal_pipeline()
+
+    def create_normal_pipeline(self):
+        components = []
+        components.append(
+            ComposableNode(
+                package="compare_map_segmentation",
+                plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
+                name="voxel_based_compare_map_filter",
+                remappings=[
+                    ("input", LaunchConfiguration("input_topic")),
+                    ("map", "/map/pointcloud_map"),
+                    ("output", LaunchConfiguration("output_topic")),
+                ],
+                parameters=[
+                    {
+                        "distance_threshold": self.distance_threshold,
+                    }
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": False},
+                ],
+            )
+        )
+        return components
+
+    def create_down_sample_pipeline(self, output_topic):
+        components = []
+        down_sample_topic = (
+            "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud"
+        )
+        components.append(
+            ComposableNode(
+                package="pointcloud_preprocessor",
+                plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+                name="voxel_grid_downsample_filter",
+                remappings=[
+                    ("input", LaunchConfiguration("input_topic")),
+                    ("output", down_sample_topic),
+                ],
+                parameters=[
+                    {
+                        "voxel_size_x": self.voxel_size,
+                        "voxel_size_y": self.voxel_size,
+                        "voxel_size_z": self.voxel_size,
+                    }
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            ),
+        )
+        components.append(
+            ComposableNode(
+                package="compare_map_segmentation",
+                plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
+                name="voxel_based_compare_map_filter",
+                remappings=[
+                    ("input", down_sample_topic),
+                    ("map", "/map/pointcloud_map"),
+                    ("output", LaunchConfiguration("output_topic")),
+                ],
+                parameters=[
+                    {
+                        "distance_threshold": self.distance_threshold,
+                    }
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": False},
+                ],
+            )
+        )
+        return components
+
+
+def launch_setup(context, *args, **kwargs):
+    pipeline = DetectionPreProcessPipeline(context)
+    components = []
+    components.extend(pipeline.create_pipeline())
+    individual_container = ComposableNodeContainer(
+        name=LaunchConfiguration("container_name"),
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=components,
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+        output="screen",
+    )
+    pointcloud_container_loader = LoadComposableNodes(
+        composable_node_descriptions=components,
+        target_container=LaunchConfiguration("container_name"),
+        condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+    return [individual_container, pointcloud_container_loader]
+
+
+def generate_launch_description():
+    launch_arguments = []
+
+    def add_launch_arg(name: str, default_value=None):
+        launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
+
+    add_launch_arg("input_topic", "")
+    add_launch_arg("output_topic", "")
+    add_launch_arg("use_multithread", "False")
+    add_launch_arg("use_intra_process", "True")
+    add_launch_arg("use_pointcloud_container", "False")
+    add_launch_arg("container_name", "detection_preprocess_pipeline_container")
+    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")),
+    )
+    return launch.LaunchDescription(
+        launch_arguments
+        + [set_container_executable, set_container_mt_executable]
+        + [OpaqueFunction(function=launch_setup)]
+    )

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -4,14 +4,18 @@
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
+  <arg name="container_name" default=""/>
 
   <!-- Pointcloud map filter -->
   <group>
-    <include file="$(find-pkg-share compare_map_segmentation)/launch/voxel_based_compare_map_filter.launch.xml" if="$(var use_pointcloud_map)">
-      <arg name="input" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="input_map" value="/map/pointcloud_map"/>
-      <arg name="output" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="distance_threshold" value="0.5"/>
+    <include file="$(find-pkg-share perception_launch)/launch/object_recognition/detection/detection_preprocess.launch.py" if="$(var use_pointcloud_map)">
+      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
+      <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
+      <arg name="use_intra_process" value="true"/>
+      <arg name="use_multithread" value="true"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="container_name" value="$(var pointcloud_container_name)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -89,6 +89,8 @@
           <arg name="camera_info7" value="$(var camera_info7)"/>
           <arg name="image_number" value="$(var image_number)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+          <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+          <arg name="container_name" value="$(var pointcloud_container_name)"/>
         </include>
       </group>
       <!-- tracking module -->