From caedccd72a7d96374aa3d8c363b3f3de45d0bbab Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@tier4.jp>
Date: Thu, 1 Feb 2024 19:44:16 +0900
Subject: [PATCH 1/4] refactor(tier4_localization_launch): use util.launch.xml
 instead of util.launch.py

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
---
 .../pose_twist_estimator.launch.xml           |   4 +-
 .../launch/util/util.launch.py                | 136 ------------------
 .../launch/util/util.launch.xml               |  39 +++++
 3 files changed, 41 insertions(+), 138 deletions(-)
 delete mode 100644 launch/tier4_localization_launch/launch/util/util.launch.py
 create mode 100644 launch/tier4_localization_launch/launch/util/util.launch.xml

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 02c6da20e17da..ce9f18de40f77 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
@@ -23,7 +23,7 @@
       <group if="$(var gnss_enabled)">
         <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
       </group>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.xml">
         <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
       </include>
     </group>
@@ -144,7 +144,7 @@
       <group if="$(var gnss_enabled)">
         <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
       </group>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.xml">
         <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
       </include>
     </group>
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py
deleted file mode 100644
index 22a45fe7b8530..0000000000000
--- a/launch/tier4_localization_launch/launch/util/util.launch.py
+++ /dev/null
@@ -1,136 +0,0 @@
-# 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.substitutions import LaunchConfiguration
-from launch_ros.actions import LoadComposableNodes
-from launch_ros.descriptions import ComposableNode
-import yaml
-
-
-def launch_setup(context, *args, **kwargs):
-    # https://github.com/ros2/launch_ros/issues/156
-    def load_composable_node_param(param_path):
-        with open(LaunchConfiguration(param_path).perform(context), "r") as f:
-            return yaml.safe_load(f)["/**"]["ros__parameters"]
-
-    crop_box_component = ComposableNode(
-        package="pointcloud_preprocessor",
-        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
-        name="crop_box_filter_measurement_range",
-        remappings=[
-            ("input", LaunchConfiguration("input_pointcloud")),
-            ("output", "measurement_range/pointcloud"),
-        ],
-        parameters=[
-            load_composable_node_param("crop_box_filter_measurement_range_param_path"),
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-    voxel_grid_downsample_component = ComposableNode(
-        package="pointcloud_preprocessor",
-        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
-        name="voxel_grid_downsample_filter",
-        remappings=[
-            ("input", "measurement_range/pointcloud"),
-            ("output", "voxel_grid_downsample/pointcloud"),
-        ],
-        parameters=[load_composable_node_param("voxel_grid_downsample_filter_param_path")],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-    random_downsample_component = ComposableNode(
-        package="pointcloud_preprocessor",
-        plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent",
-        name="random_downsample_filter",
-        remappings=[
-            ("input", "voxel_grid_downsample/pointcloud"),
-            ("output", LaunchConfiguration("output/pointcloud")),
-        ],
-        parameters=[load_composable_node_param("random_downsample_filter_param_path")],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
-    composable_nodes = [
-        crop_box_component,
-        voxel_grid_downsample_component,
-        random_downsample_component,
-    ]
-
-    load_composable_nodes = LoadComposableNodes(
-        composable_node_descriptions=composable_nodes,
-        target_container=LaunchConfiguration("lidar_container_name"),
-    )
-
-    return [load_composable_nodes]
-
-
-def generate_launch_description():
-    launch_arguments = []
-
-    def add_launch_arg(name: str, default_value=None, description=None):
-        arg = DeclareLaunchArgument(name, default_value=default_value, description=description)
-        launch_arguments.append(arg)
-
-    add_launch_arg(
-        "crop_box_filter_measurement_range_param_path",
-        [
-            LaunchConfiguration("crop_box_filter_measurement_range_param_path"),
-        ],
-        "path to the parameter file of crop_box_filter_measurement_range",
-    )
-    add_launch_arg(
-        "voxel_grid_downsample_filter_param_path",
-        [
-            LaunchConfiguration("voxel_grid_downsample_filter_param_path"),
-        ],
-        "path to the parameter file of voxel_grid_downsample_filter",
-    )
-    add_launch_arg(
-        "random_downsample_filter_param_path",
-        [
-            LaunchConfiguration("random_downsample_filter_param_path"),
-        ],
-        "path to the parameter file of random_downsample_filter",
-    )
-    add_launch_arg("use_intra_process", "true", "use ROS 2 component container communication")
-    add_launch_arg(
-        "lidar_container_name",
-        "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container",
-        "container name of main lidar used for localization",
-    )
-
-    add_launch_arg(
-        "output/pointcloud",
-        "downsample/pointcloud",
-        "final output topic name",
-    )
-    add_launch_arg(
-        "output_measurement_range_sensor_points_topic",
-        "measurement_range/pointcloud",
-        "output topic name for crop box filter",
-    )
-    add_launch_arg(
-        "output_voxel_grid_downsample_sensor_points_topic",
-        "voxel_grid_downsample/pointcloud",
-        "output topic name for voxel grid downsample filter",
-    )
-    add_launch_arg(
-        "output_downsample_sensor_points_topic",
-        "downsample/pointcloud",
-        "output topic name for downsample filter. this is final output",
-    )
-
-    return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
new file mode 100644
index 0000000000000..de568a30643e9
--- /dev/null
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -0,0 +1,39 @@
+<launch>
+
+  <!-- topic -->
+  <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"/>
+
+  <!-- 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)">
+
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
+      <param from="$(var crop_box_filter_measurement_range_param_path)"/>
+      <remap from="input" to="$(var input_pointcloud)"/>
+      <remap from="output" to="measurement_range/pointcloud"/>
+      <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+    </composable_node>
+
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" name="voxel_grid_downsample_filter">
+      <param from="$(var voxel_grid_downsample_filter_param_path)"/>
+      <remap from="input" to="measurement_range/pointcloud"/>
+      <remap from="output" to="voxel_grid_downsample/pointcloud"/>
+      <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+    </composable_node>
+
+
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent" name="random_downsample_filter">
+      <param from="$(var random_downsample_filter_param_path)"/>
+      <remap from="input" to="voxel_grid_downsample/pointcloud"/>
+      <remap from="output" to="$(var output/pointcloud)"/>
+      <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+    </composable_node>
+
+  </load_composable_node>
+
+</launch>

From 7fd55cf42b7439b40080a4bbe1a96f9e27214f60 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 1 Feb 2024 10:55:08 +0000
Subject: [PATCH 2/4] style(pre-commit): autofix

---
 .../tier4_localization_launch/launch/util/util.launch.xml   | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
index de568a30643e9..4d0f7505d16a6 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -1,5 +1,4 @@
 <launch>
-
   <!-- topic -->
   <arg name="output/pointcloud" default="downsample/pointcloud" description="final output topic name"/>
 
@@ -9,9 +8,7 @@
   <!-- 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)">
-
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
       <param from="$(var crop_box_filter_measurement_range_param_path)"/>
       <remap from="input" to="$(var input_pointcloud)"/>
@@ -26,14 +23,11 @@
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
     </composable_node>
 
-
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent" name="random_downsample_filter">
       <param from="$(var random_downsample_filter_param_path)"/>
       <remap from="input" to="voxel_grid_downsample/pointcloud"/>
       <remap from="output" to="$(var output/pointcloud)"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
     </composable_node>
-
   </load_composable_node>
-
 </launch>

From b7c8f1a297afd3ca697b75abec32579cb5488c84 Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@tier4.jp>
Date: Tue, 6 Feb 2024 17:06:44 +0900
Subject: [PATCH 3/4] chore(ndt_scan_matcher): rename config path

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
---
 .../launch/localization.launch.xml                        | 8 ++++----
 .../pose_twist_estimator/ndt_scan_matcher.launch.xml      | 2 +-
 .../tier4_localization_launch/launch/util/util.launch.xml | 6 +++---
 3 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
index 9f51f7e6ea7a1..d178b8bb16ac7 100644
--- a/launch/tier4_localization_launch/launch/localization.launch.xml
+++ b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -5,10 +5,10 @@
   <arg name="system_run_mode"/>
 
   <!-- Parameter files -->
-  <arg name="crop_box_filter_measurement_range_param_path"/>
-  <arg name="voxel_grid_downsample_filter_param_path"/>
-  <arg name="random_downsample_filter_param_path"/>
-  <arg name="ndt_scan_matcher_param_path"/>
+  <arg name="ndt_scan_matcher/pointcloud_preprocess/crop_box_filter_measurement_range_param_path"/>
+  <arg name="ndt_scan_matcher/pointcloud_preprocess/voxel_grid_downsample_filter_param_path"/>
+  <arg name="ndt_scan_matcher/pointcloud_preprocess/random_downsample_filter_param_path"/>
+  <arg name="ndt_scan_matcher/ndt_scan_matcher_param_path"/>
   <arg name="localization_error_monitor_param_path"/>
   <arg name="ekf_localizer_param_path"/>
   <arg name="pose_initializer_param_path"/>
diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml
index bee300c587816..686e6d6cf12bc 100644
--- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml
@@ -12,7 +12,7 @@
 
       <arg name="client_map_loader" value="/map/get_differential_pointcloud_map"/>
 
-      <arg name="param_file" value="$(var ndt_scan_matcher_param_path)"/>
+      <arg name="param_file" value="$(var ndt_scan_matcher/ndt_scan_matcher_param_path)"/>
     </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 4d0f7505d16a6..350ddd8a4c46b 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -10,21 +10,21 @@
 
   <load_composable_node target="$(var lidar_container_name)">
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
-      <param from="$(var crop_box_filter_measurement_range_param_path)"/>
+      <param from="$(var ndt_scan_matcher/pointcloud_preprocess/crop_box_filter_measurement_range_param_path)"/>
       <remap from="input" to="$(var input_pointcloud)"/>
       <remap from="output" to="measurement_range/pointcloud"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
     </composable_node>
 
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" name="voxel_grid_downsample_filter">
-      <param from="$(var voxel_grid_downsample_filter_param_path)"/>
+      <param from="$(var ndt_scan_matcher/pointcloud_preprocess/voxel_grid_downsample_filter_param_path)"/>
       <remap from="input" to="measurement_range/pointcloud"/>
       <remap from="output" to="voxel_grid_downsample/pointcloud"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
     </composable_node>
 
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent" name="random_downsample_filter">
-      <param from="$(var random_downsample_filter_param_path)"/>
+      <param from="$(var ndt_scan_matcher/pointcloud_preprocess/random_downsample_filter_param_path)"/>
       <remap from="input" to="voxel_grid_downsample/pointcloud"/>
       <remap from="output" to="$(var output/pointcloud)"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>

From b8e779dd26fe81b04b4c297703bbefc52ca18416 Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@tier4.jp>
Date: Tue, 6 Feb 2024 17:58:06 +0900
Subject: [PATCH 4/4] rename path

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
---
 .../launch/localization.launch.xml                     | 10 ++++++----
 .../launch/util/util.launch.xml                        |  6 +++---
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
index d178b8bb16ac7..b9cc00c9b35a5 100644
--- a/launch/tier4_localization_launch/launch/localization.launch.xml
+++ b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -4,11 +4,13 @@
   <arg name="twist_source"/>
   <arg name="system_run_mode"/>
 
-  <!-- Parameter files -->
-  <arg name="ndt_scan_matcher/pointcloud_preprocess/crop_box_filter_measurement_range_param_path"/>
-  <arg name="ndt_scan_matcher/pointcloud_preprocess/voxel_grid_downsample_filter_param_path"/>
-  <arg name="ndt_scan_matcher/pointcloud_preprocess/random_downsample_filter_param_path"/>
+  <!-- parameter paths for ndt -->
+  <arg name="ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"/>
+  <arg name="ndt_scan_matcher/pointcloud_preprocessor/voxel_grid_downsample_filter_param_path"/>
+  <arg name="ndt_scan_matcher/pointcloud_preprocessor/random_downsample_filter_param_path"/>
   <arg name="ndt_scan_matcher/ndt_scan_matcher_param_path"/>
+
+  <!-- Parameter files -->
   <arg name="localization_error_monitor_param_path"/>
   <arg name="ekf_localizer_param_path"/>
   <arg name="pose_initializer_param_path"/>
diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml
index 350ddd8a4c46b..aeba276308646 100644
--- a/launch/tier4_localization_launch/launch/util/util.launch.xml
+++ b/launch/tier4_localization_launch/launch/util/util.launch.xml
@@ -10,21 +10,21 @@
 
   <load_composable_node target="$(var lidar_container_name)">
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
-      <param from="$(var ndt_scan_matcher/pointcloud_preprocess/crop_box_filter_measurement_range_param_path)"/>
+      <param from="$(var ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path)"/>
       <remap from="input" to="$(var input_pointcloud)"/>
       <remap from="output" to="measurement_range/pointcloud"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
     </composable_node>
 
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" name="voxel_grid_downsample_filter">
-      <param from="$(var ndt_scan_matcher/pointcloud_preprocess/voxel_grid_downsample_filter_param_path)"/>
+      <param from="$(var ndt_scan_matcher/pointcloud_preprocessor/voxel_grid_downsample_filter_param_path)"/>
       <remap from="input" to="measurement_range/pointcloud"/>
       <remap from="output" to="voxel_grid_downsample/pointcloud"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
     </composable_node>
 
     <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent" name="random_downsample_filter">
-      <param from="$(var ndt_scan_matcher/pointcloud_preprocess/random_downsample_filter_param_path)"/>
+      <param from="$(var ndt_scan_matcher/pointcloud_preprocessor/random_downsample_filter_param_path)"/>
       <remap from="input" to="voxel_grid_downsample/pointcloud"/>
       <remap from="output" to="$(var output/pointcloud)"/>
       <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>