From c139096d2c8beb80c03dec56c10f8b86b532155a Mon Sep 17 00:00:00 2001
From: Xinyu Wang <xinyu.wang@tier4.jp>
Date: Tue, 5 Jul 2022 14:52:49 +0900
Subject: [PATCH 1/2] fix(tier4_localization_launch): add group tag

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>
---
 .../launch/localization.launch.xml            |  4 +-
 .../localization_error_monitor.launch.xml     | 18 +++---
 .../pose_estimator/pose_estimator.launch.xml  | 18 +++---
 .../pose_twist_fusion_filter.launch.xml       | 64 ++++++++++---------
 .../twist_estimator.launch.xml                | 12 ++--
 .../launch/util/util.launch.xml               | 15 +++--
 6 files changed, 74 insertions(+), 57 deletions(-)

diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
index 46c06d679cdcc..e1ebef8f9b170 100644
--- a/launch/tier4_localization_launch/launch/localization.launch.xml
+++ b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -28,6 +28,8 @@
       <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
     </group>
     <!-- localization_error_monitor_module -->
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/localization_error_monitor/localization_error_monitor.launch.xml"/>
+    <group>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/localization_error_monitor/localization_error_monitor.launch.xml"/>
+    </group>
   </group>
 </launch>
diff --git a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml
index f7f356ee1b444..790e4cc2fe1b1 100644
--- a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml
+++ b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml
@@ -1,11 +1,13 @@
 <?xml version="1.0"?>
 <launch>
-  <include file="$(find-pkg-share localization_error_monitor)/launch/localization_error_monitor.launch.xml">
-    <arg name="input/pose_with_cov" value="/localization/pose_with_covariance"/>
-    <arg name="scale" value="3.0"/>
-    <arg name="error_ellipse_size" value="1.0"/>
-    <arg name="warn_ellipse_size" value="0.8"/>
-    <arg name="error_ellipse_size_lateral_direction" value="1.0"/>
-    <arg name="warn_ellipse_size_lateral_direction" value="0.8"/>
-  </include>
+  <group>
+    <include file="$(find-pkg-share localization_error_monitor)/launch/localization_error_monitor.launch.xml">
+      <arg name="input/pose_with_cov" value="/localization/pose_with_covariance"/>
+      <arg name="scale" value="3.0"/>
+      <arg name="error_ellipse_size" value="1.0"/>
+      <arg name="warn_ellipse_size" value="0.8"/>
+      <arg name="error_ellipse_size_lateral_direction" value="1.0"/>
+      <arg name="warn_ellipse_size_lateral_direction" value="0.8"/>
+    </include>
+  </group>
 </launch>
diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml
index 773ef64fdf40f..7ea99cb22878f 100644
--- a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml
@@ -1,13 +1,15 @@
 <?xml version="1.0"?>
 <launch>
-  <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
-    <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
-    <arg name="input/pointcloud" value="/localization/util/downsample/pointcloud"/>
-    <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"/>
+  <group>
+    <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
+      <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
+      <arg name="input/pointcloud" value="/localization/util/downsample/pointcloud"/>
+      <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"/>
 
-    <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
-    <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
+      <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
 
-    <arg name="param_file" value="$(find-pkg-share tier4_localization_launch)/config/ndt_scan_matcher.param.yaml"/>
-  </include>
+      <arg name="param_file" value="$(find-pkg-share tier4_localization_launch)/config/ndt_scan_matcher.param.yaml"/>
+    </include>
+  </group>
 </launch>
diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
index 3aaafe9ac2254..12ccdf0ad287f 100644
--- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
@@ -1,34 +1,40 @@
 <?xml version="1.0"?>
 <launch>
-  <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
-    <arg name="enable_yaw_bias_estimation" value="False"/>
-    <arg name="tf_rate" value="50.0"/>
-    <arg name="twist_smoothing_steps" value="2"/>
-    <arg name="input_initial_pose_name" value="/initialpose3d"/>
+  <group>
+    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
+      <arg name="enable_yaw_bias_estimation" value="False"/>
+      <arg name="tf_rate" value="50.0"/>
+      <arg name="twist_smoothing_steps" value="2"/>
+      <arg name="input_initial_pose_name" value="/initialpose3d"/>
+      <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="output_odom_name" value="kinematic_state"/>
+      <arg name="output_pose_name" value="pose"/>
+      <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
+      <arg name="output_pose_without_yawbias_name" value="pose_without_yawbias"/>
+      <arg name="output_pose_with_covariance_without_yawbias_name" value="pose_with_covariance_without_yawbias"/>
+      <arg name="output_twist_name" value="twist"/>
+      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
+      <arg name="proc_stddev_vx_c" value="10.0"/>
+      <arg name="proc_stddev_wz_c" value="5.0"/>
+    </include>
+  </group>
 
-    <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
-    <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
-    <arg name="output_odom_name" value="kinematic_state"/>
-    <arg name="output_pose_name" value="pose"/>
-    <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
-    <arg name="output_pose_without_yawbias_name" value="pose_without_yawbias"/>
-    <arg name="output_pose_with_covariance_without_yawbias_name" value="pose_with_covariance_without_yawbias"/>
-    <arg name="output_twist_name" value="twist"/>
-    <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
+  <group>
+    <include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
+      <arg name="use_twist_with_covariance" value="True"/>
+      <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
+      <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
+      <arg name="output_odom_name" value="/localization/kinematic_state"/>
+    </include>
+  </group>
 
-    <arg name="proc_stddev_vx_c" value="10.0"/>
-    <arg name="proc_stddev_wz_c" value="5.0"/>
-  </include>
-  <include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
-    <arg name="use_twist_with_covariance" value="True"/>
-    <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
-    <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
-    <arg name="output_odom_name" value="/localization/kinematic_state"/>
-  </include>
-  <include file="$(find-pkg-share twist2accel)/launch/twist2accel.launch.xml">
-    <arg name="use_odom" value="true"/>
-    <arg name="in_odom" value="/localization/kinematic_state"/>
-    <arg name="in_twist" value="/localization/twist_estimator/twist_with_covariance"/>
-    <arg name="out_accel" value="/localization/acceleration"/>
-  </include>
+  <group>
+    <include file="$(find-pkg-share twist2accel)/launch/twist2accel.launch.xml">
+      <arg name="use_odom" value="true"/>
+      <arg name="in_odom" value="/localization/kinematic_state"/>
+      <arg name="in_twist" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="out_accel" value="/localization/acceleration"/>
+    </include>
+  </group>
 </launch>
diff --git a/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml
index 155521a0edef0..3627c6698f4bd 100644
--- a/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml
+++ b/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml
@@ -1,8 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
-    <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
-    <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
-    <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
-  </include>
+  <group>
+    <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
+      <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
+    </include>
+  </group>
 </launch>
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
index 6053b7c7be25f..d1a4ea7c49161 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -11,11 +11,14 @@
   <arg name="use_intra_process" default="true" description="use ROS2 component container communication"/>
 
   <!-- pose_initializer -->
-  <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"/>
-
+  <group>
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"/>
+  </group>
   <!-- util -->
-  <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
-    <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-    <arg name="output/pointcloud" value="$(var output/pointcloud)"/>
-  </include>
+  <group>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="output/pointcloud" value="$(var output/pointcloud)"/>
+    </include>
+  </group>
 </launch>

From 5ce6314675f17373e76e23c1c35c758eafdd6596 Mon Sep 17 00:00:00 2001
From: Xinyu Wang <xinyu.wang@tier4.jp>
Date: Tue, 12 Jul 2022 14:28:04 +0900
Subject: [PATCH 2/2] add more args into group

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>
---
 launch/tier4_localization_launch/launch/util/util.launch.xml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
index d1a4ea7c49161..9c46d606fe523 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -19,6 +19,8 @@
     <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="output/pointcloud" value="$(var output/pointcloud)"/>
+      <arg name="container" value="$(var container)"/>
+      <arg name="use_intra_process" value="$(var use_intra_process)"/>
     </include>
   </group>
 </launch>