From 42caf95b4501cdcf456415e07a5107d6d82985a1 Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Thu, 25 Apr 2024 13:48:33 +0900
Subject: [PATCH 1/5] feat: downsample perception input pointcloud

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../launch/perception.launch.xml              | 23 ++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 822376b365fa8..b90fd828ea9b0 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -120,6 +120,23 @@
     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_input_pointcloud" default="false"/>
+  <arg name="downsample_voxel_size" default="0.05"/>
+  <let name="downsampled_pointcloud" value="/perception/input/downsampled/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)"/>
+  <load_composable_node target="$(var pointcloud_container_name)">
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" name="perception_input_pc_downsampler" namespace="">
+      <remap from="input" to="$(var input/pointcloud)"/>
+      <remap from="output" to="$(var downsampled_pointcloud)"/>
+      <param name="voxel_size_x" value="$(var downsample_voxel_size)"/>
+      <param name="voxel_size_y" value="$(var downsample_voxel_size)"/>
+      <param name="voxel_size_z" value="$(var downsample_voxel_size)"/>
+      <extra_arg name="use_intra_process_comms" value="true"/>
+    </composable_node>
+  </load_composable_node>
+
   <!-- Perception module -->
   <group>
     <push-ros-namespace namespace="perception"/>
@@ -132,7 +149,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>
 
@@ -141,7 +158,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"/>
@@ -161,7 +178,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)"/>

From c77bb73d7638cf5f7a454dc8c50a201b4b30786c Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Fri, 26 Apr 2024 12:22:46 +0900
Subject: [PATCH 2/5] fix: add group if to switch downsample node

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../launch/perception.launch.xml              | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index b90fd828ea9b0..9cf43ca2d4f8e 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -126,16 +126,18 @@
   <let name="downsampled_pointcloud" value="/perception/input/downsampled/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)"/>
-  <load_composable_node target="$(var pointcloud_container_name)">
-    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" name="perception_input_pc_downsampler" namespace="">
-      <remap from="input" to="$(var input/pointcloud)"/>
-      <remap from="output" to="$(var downsampled_pointcloud)"/>
-      <param name="voxel_size_x" value="$(var downsample_voxel_size)"/>
-      <param name="voxel_size_y" value="$(var downsample_voxel_size)"/>
-      <param name="voxel_size_z" value="$(var downsample_voxel_size)"/>
-      <extra_arg name="use_intra_process_comms" value="true"/>
-    </composable_node>
-  </load_composable_node>
+  <group if="$(var downsample_input_pointcloud)">
+    <load_composable_node target="$(var pointcloud_container_name)">
+      <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" name="perception_input_pc_downsample_node" namespace="">
+        <remap from="input" to="$(var input/pointcloud)"/>
+        <remap from="output" to="$(var downsampled_pointcloud)"/>
+        <param name="voxel_size_x" value="$(var downsample_voxel_size)"/>
+        <param name="voxel_size_y" value="$(var downsample_voxel_size)"/>
+        <param name="voxel_size_z" value="$(var downsample_voxel_size)"/>
+        <extra_arg name="use_intra_process_comms" value="true"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
 
   <!-- Perception module -->
   <group>

From ca654d281c82dc3345058fa8cda909772172ba00 Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Fri, 26 Apr 2024 20:13:27 +0900
Subject: [PATCH 3/5] fix: add test and exec depend

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 perception/probabilistic_occupancy_grid_map/package.xml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml
index a90ae78b03597..3efed07535504 100644
--- a/perception/probabilistic_occupancy_grid_map/package.xml
+++ b/perception/probabilistic_occupancy_grid_map/package.xml
@@ -34,6 +34,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
+  <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pointcloud_to_laserscan</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
@@ -44,6 +45,7 @@
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>pointcloud_preprocessor</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

From 961407cc9b5e6db7cbb6a9400a685b4a33b9106d Mon Sep 17 00:00:00 2001
From: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
Date: Sat, 27 Apr 2024 17:37:32 +0900
Subject: [PATCH 4/5] Update
 launch/tier4_perception_launch/launch/perception.launch.xml

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
---
 launch/tier4_perception_launch/launch/perception.launch.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 9cf43ca2d4f8e..39b1cdb1c2645 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -123,7 +123,7 @@
   <!-- Downsample pointcloud for perception usage -->
   <arg name="downsample_input_pointcloud" default="false"/>
   <arg name="downsample_voxel_size" default="0.05"/>
-  <let name="downsampled_pointcloud" value="/perception/input/downsampled/pointcloud"/>
+  <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)">

From d07ee84cc47bc9c1419a531c94015d80789e0f07 Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Mon, 13 May 2024 16:44:04 +0900
Subject: [PATCH 5/5] chore: refactor perception.launch.xml

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../launch/perception.launch.xml              | 38 ++++++++++---------
 1 file changed, 21 insertions(+), 17 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index ac8da0f5cc78a..d020bfe0d6786 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -122,27 +122,31 @@
   />
 
   <!-- Downsample pointcloud for perception usage -->
-  <arg name="downsample_input_pointcloud" default="false"/>
-  <arg name="downsample_voxel_size" default="0.05"/>
-  <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)">
-    <load_composable_node target="$(var pointcloud_container_name)">
-      <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" name="perception_input_pc_downsample_node" namespace="">
-        <remap from="input" to="$(var input/pointcloud)"/>
-        <remap from="output" to="$(var downsampled_pointcloud)"/>
-        <param name="voxel_size_x" value="$(var downsample_voxel_size)"/>
-        <param name="voxel_size_y" value="$(var downsample_voxel_size)"/>
-        <param name="voxel_size_z" value="$(var downsample_voxel_size)"/>
-        <extra_arg name="use_intra_process_comms" value="true"/>
-      </composable_node>
-    </load_composable_node>
-  </group>
+  <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>