From 5cbbead7da79cc628e471e5ae1e766602b4f4302 Mon Sep 17 00:00:00 2001
From: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
Date: Tue, 14 May 2024 01:13:18 +0900
Subject: [PATCH] feat(tier4_perception_launch): downsample perception input
 pointcloud (#6886)

* feat: downsample perception input pointcloud

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix: add group if to switch downsample node

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix: add test and exec depend

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* Update launch/tier4_perception_launch/launch/perception.launch.xml

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

* chore: refactor perception.launch.xml

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Signed-off-by: vividf <yihsiang.fang@tier4.jp>
---
 .../launch/perception.launch.xml              | 29 +++++++++++++++++--
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 71fb38f88314..d020bfe0d678 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -121,9 +121,32 @@
     description="if use_radar_tracking_fusion:=true, radar information is merged in tracking launch. Otherwise, radar information is merged in detection launch."
   />
 
+  <!-- Downsample pointcloud for perception usage -->
+  <arg name="downsample_perception_common_pointcloud" default="false"/>
+  <arg name="common_downsample_voxel_size_x" default="0.05"/>
+  <arg name="common_downsample_voxel_size_y" default="0.05"/>
+  <arg name="common_downsample_voxel_size_z" default="0.05"/>
+
   <!-- Perception module -->
   <group>
     <push-ros-namespace namespace="perception"/>
+    <!-- Perception common preprocess -->
+    <let name="downsampled_pointcloud" value="/perception/common/pointcloud"/>
+    <let name="perception_pointcloud" value="$(var input/pointcloud)" unless="$(var downsample_input_pointcloud)"/>
+    <let name="perception_pointcloud" value="$(var downsampled_pointcloud)" if="$(var downsample_input_pointcloud)"/>
+    <group if="$(var downsample_input_pointcloud)">
+      <push-ros-namespace namespace="common"/>
+      <load_composable_node target="$(var pointcloud_container_name)">
+        <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" name="pointcloud_downsample_node" namespace="">
+          <remap from="input" to="$(var input/pointcloud)"/>
+          <remap from="output" to="$(var downsampled_pointcloud)"/>
+          <param name="voxel_size_x" value="$(var common_downsample_voxel_size_x)"/>
+          <param name="voxel_size_y" value="$(var common_downsample_voxel_size_y)"/>
+          <param name="voxel_size_z" value="$(var common_downsample_voxel_size_z)"/>
+          <extra_arg name="use_intra_process_comms" value="true"/>
+        </composable_node>
+      </load_composable_node>
+    </group>
 
     <!-- Object segmentation module -->
     <group>
@@ -133,7 +156,7 @@
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
         <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
-        <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="input/pointcloud" value="$(var perception_pointcloud)"/>
       </include>
     </group>
 
@@ -142,7 +165,7 @@
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud"/>
-        <arg name="input/raw_pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="input/raw_pointcloud" value="$(var perception_pointcloud)"/>
         <arg name="output" value="/perception/occupancy_grid_map/map"/>
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
@@ -162,7 +185,7 @@
         <push-ros-namespace namespace="detection"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detection.launch.xml">
           <arg name="mode" value="$(var mode)"/>
-          <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+          <arg name="input/pointcloud" value="$(var perception_pointcloud)"/>
           <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
           <arg name="image_raw0" value="$(var image_raw0)"/>
           <arg name="camera_info0" value="$(var camera_info0)"/>