feat: use voxel_grid_downsample_filter for centerpoint input pointcloud

Signed-off-by: Shin-kyoto <aquashin0202@gmail.com>

perception/lidar_centerpoint/launch/centerpoint_preprocess.launch.py

@@ -0,0 +1,109 @@
+# Copyright 2020 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 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
+
+
+class LidarCenterPointPreProcessPipeline:
+    def __init__(self, context):
+        pass
+
+    def create_pipeline(self):
+        components = []
+        components.append(
+            ComposableNode(
+                package="pointcloud_preprocessor",
+                plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+                name="voxel_grid_downsample_filter",
+                remappings=[
+                    ("input", LaunchConfiguration("input_topic")),
+                    ("output", LaunchConfiguration("output_topic")),
+                ],
+                parameters=[
+                    {
+                        "voxel_size_x": 0.1,
+                        "voxel_size_y": 0.1,
+                        "voxel_size_z": 0.1,
+                    }
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
+        )
+
+        return components
+
+
+def launch_setup(context, *args, **kwargs):
+    pipeline = LidarCenterPointPreProcessPipeline(context)
+
+    components = 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", "centerpoint_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)]
+    )

perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml

@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="input/pointcloud" default="/sensing/lidar/pointcloud"/>
+  <arg name="input/pointcloud/filtered" default="/sensing/lidar/concatenated/filtered/pointcloud"/>
   <arg name="output/objects" default="objects"/>
   <arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
   <arg name="model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
@@ -11,8 +12,16 @@
   <arg name="has_twist" default="false"/>
   <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
 
+  <include file="$(find-pkg-share lidar_centerpoint)/launch/centerpoint_preprocess.launch.py">
+    <arg name="input_topic" value="$(var input/pointcloud)"/>
+    <arg name="output_topic" value="$(var input/pointcloud/filtered)"/>
+    <arg name="use_multithread" value="False"/>
+    <arg name="use_intra_process" value="True"/>
+    <arg name="use_pointcloud_container" value="False"/>
+  </include>
+
   <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
-    <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+    <remap from="~/input/pointcloud" to="$(var input/pointcloud/filtered)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
     <param name="score_threshold" value="$(var score_threshold)"/>
     <param name="densification_world_frame_id" value="map"/>