From ee328852a1dd20bd6be2b7699c49aa11b1d741ed Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <taiki.yamada@tier4.jp>
Date: Tue, 27 Feb 2024 13:35:25 +0900
Subject: [PATCH] Create pointcloud_container for preprocessing in localization
 component

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 .../launch/localization.launch.xml                         | 4 ++--
 .../pose_twist_estimator/pose_twist_estimator.launch.xml   | 1 -
 .../tier4_localization_launch/launch/util/util.launch.xml  | 7 +++++--
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
index 9d3fba05b4a6e..97451d2c60b35 100644
--- a/launch/tier4_localization_launch/launch/localization.launch.xml
+++ b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -18,14 +18,14 @@
   <arg name="ar_tag_based_localizer_param_path"/>
 
   <arg name="input_pointcloud" default="/sensing/lidar/top/pointcloud"/>
-  <arg name="lidar_container_name" default="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"/>
+  <arg name="target_pointcloud_container_name" default="/localization/util/pointcloud_preprocessor/pointcloud_container" description="The name of the container preprocessing the pointcloud"/>
 
   <!-- localization module -->
   <group>
     <push-ros-namespace namespace="localization"/>
     <!-- pose_twist_estimator module -->
     <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/pose_twist_estimator.launch.xml">
-      <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
+      <arg name="target_pointcloud_container_name" value="$(var target_pointcloud_container_name)"/>
     </include>
 
     <!-- pose_twist_fusion_filter module -->
diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
index c00c90d467090..9f18e89b41e55 100644
--- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
@@ -109,7 +109,6 @@
     <let name="override_input_pointcloud" value="$(var input_pointcloud)"/>
     <let name="override_input_pointcloud" value="$(var input_pointcloud)/relay" if="$(var multi_localizer_mode)"/>
     <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.xml" if="$(var use_ndt_pose)">
-      <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
       <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
     </include>
   </group>
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
index aeba276308646..3c2a6f8bc35ca 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -3,12 +3,15 @@
   <arg name="output/pointcloud" default="downsample/pointcloud" description="final output topic name"/>
 
   <!-- container -->
-  <arg name="lidar_container_name" default="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container" description="container name of main lidar used for localization"/>
+  <arg name="target_pointcloud_container_name" default="/localization/util/pointcloud_preprocessor/pointcloud_container" description="container name of the pointcloud used for localization"/>
+  <let name="use_existing_pointcloud_container" value="$(eval &quot;'$(var target_pointcloud_container_name)' != '/localization/util/pointcloud_preprocessor/pointcloud_container' &quot;)"/>
+
+  <node_container unless="$(var use_existing_pointcloud_container)" pkg="rclcpp_components" exec="component_container" name="pointcloud_container" namespace="pointcloud_preprocessor"/>
 
   <!-- whether use intra-process -->
   <arg name="use_intra_process" default="true" description="use ROS 2 component container communication"/>
 
-  <load_composable_node target="$(var lidar_container_name)">
+  <load_composable_node target="$(var target_pointcloud_container_name)">
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
       <param from="$(var ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path)"/>
       <remap from="input" to="$(var input_pointcloud)"/>