From 013c257f2e45455dfb3323e8cb8643285ff71a54 Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Wed, 7 Feb 2024 11:22:59 +0900
Subject: [PATCH 1/2] feat: try to move updater param description to yaml file

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 ...robabilistic_occupancy_grid_map.launch.xml |  8 -----
 .../launch/perception.launch.xml              |  4 ---
 .../launch/simulator.launch.xml               |  4 ---
 .../occupancy_grid_based_validator.launch.xml |  3 --
 ...erscan_based_occupancy_grid_map.param.yaml |  3 ++
 ...tcloud_based_occupancy_grid_map.param.yaml |  3 ++
 ...serscan_based_occupancy_grid_map.launch.py | 34 ++++++++++++-------
 ...ntcloud_based_occupancy_grid_map.launch.py | 34 ++++++++++++-------
 8 files changed, 50 insertions(+), 43 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml
index 244e0940acb70..676bd67def19b 100644
--- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml
+++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml
@@ -10,8 +10,6 @@
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name="occupancy_grid_map_method" description="options: pointcloud_based_occupancy_grid_map, laserscan_based_occupancy_grid_map, multi_lidar_pointcloud_based_occupancy_grid_map"/>
   <arg name="occupancy_grid_map_param_path"/>
-  <arg name="occupancy_grid_map_updater" description="options: binary_bayes_filter"/>
-  <arg name="occupancy_grid_map_updater_param_path"/>
   <arg name="input_obstacle_pointcloud" default="false" description="subscribe obstacle pointcloud"/>
   <arg name="input_obstacle_and_raw_pointcloud" default="true" description="subscribe both obstacle and raw pointcloud"/>
 
@@ -26,8 +24,6 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       <arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
-      <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
-      <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
     </include>
   </group>
 
@@ -42,8 +38,6 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       <arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
-      <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
-      <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
       <arg name="input_obstacle_pointcloud" value="$(var input_obstacle_pointcloud)"/>
       <arg name="input_obstacle_and_raw_pointcloud" value="$(var input_obstacle_and_raw_pointcloud)"/>
     </include>
@@ -58,8 +52,6 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       <arg name="multi_lidar_fusion_config_file" value="$(var occupancy_grid_map_param_path)"/>
-      <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
-      <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 0d517ced97d17..ad7aa9c52fe83 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -31,8 +31,6 @@
   <arg name="object_recognition_detection_detection_by_tracker_param"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
-  <arg name="occupancy_grid_map_updater"/>
-  <arg name="occupancy_grid_map_updater_param_path"/>
   <arg name="traffic_light_arbiter_param_path"/>
 
   <!-- CenterPoint model parameters -->
@@ -150,8 +148,6 @@
         <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
         <arg name="occupancy_grid_map_method" value="$(var occupancy_grid_map_method)"/>
         <arg name="occupancy_grid_map_param_path" value="$(var occupancy_grid_map_param_path)"/>
-        <arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
-        <arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
       </include>
     </group>
 
diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 4da6720b0af47..bf34eea4bf341 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -3,8 +3,6 @@
   <arg name="fault_injection_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="laserscan_based_occupancy_grid_map_param_path"/>
-  <arg name="occupancy_grid_map_updater"/>
-  <arg name="occupancy_grid_map_updater_param_path"/>
   <arg name="pose_initializer_param_path"/>
 
   <arg name="launch_dummy_perception"/>
@@ -51,8 +49,6 @@
       <arg name="output" value="/perception/occupancy_grid_map/map"/>
       <arg name="occupancy_grid_map_method" value="laserscan_based_occupancy_grid_map"/>
       <arg name="occupancy_grid_map_param_path" value="$(var laserscan_based_occupancy_grid_map_param_path)"/>
-      <arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
-      <arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
     </include>
   </group>
 
diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml
index 3acb1f2d1907a..158c067774218 100644
--- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml
+++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml
@@ -6,7 +6,6 @@
   <arg name="input/laserscan" default="/perception/occupancy_grid_map/virtual_scan/laserscan"/>
   <arg name="input/occupancy_grid_map" default="/perception/object_recognition/detection/validation/occupancy_grid_based/single_frame_occupancy_grid_map"/>
   <arg name="occupancy_grid_map_param_path" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/laserscan_based_occupancy_grid_map.param.yaml"/>
-  <arg name="occupancy_grid_map_updater_param_path" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/binary_bayes_filter_updater.param.yaml"/>
   <arg name="occupancy_grid_based_validator_param_path" default="$(find-pkg-share detected_object_validation)/config/occupancy_grid_based_validator_param_path.param.yaml"/>
 
   <node pkg="probabilistic_occupancy_grid_map" exec="laserscan_based_occupancy_grid_map_node" name="single_frame_laserscan_occupancy_grid_map" output="screen">
@@ -14,9 +13,7 @@
     <remap from="~/output/occupancy_grid_map" to="$(var input/occupancy_grid_map)"/>
     <param name="input_obstacle_pointcloud" value="false"/>
     <param name="input_obstacle_and_raw_pointcloud" value="false"/>
-    <param name="updater_type" value="binary_bayes_filter"/>
     <param from="$(var occupancy_grid_map_param_path)"/>
-    <param from="$(var occupancy_grid_map_updater_param_path)"/>
   </node>
 
   <node pkg="detected_object_validation" exec="occupancy_grid_based_validator_node" name="occupancy_grid_based_validator_node" output="screen">
diff --git a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml
index 9dcc722587e92..b3f7881c86a7a 100644
--- a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml
+++ b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml
@@ -16,3 +16,6 @@
     map_length: 150.0
     map_width: 150.0
     map_resolution: 0.5
+
+    updater_type: "binary_bayes_filter"
+    updater_param_file: $(find-pkg-share probabilistic_occupancy_grid_map)/config/binary_bayes_filter.param.yaml
diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
index 8077bdec5008e..9722aa442a8ec 100644
--- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
+++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml
@@ -21,6 +21,9 @@
     # base_link should not be used with "OccupancyGridMapProjectiveBlindSpot"
     scan_origin_frame: "base_link"
 
+    updater_type: "binary_bayes_filter"
+    updater_param_file: $(find-pkg-share probabilistic_occupancy_grid_map)/config/binary_bayes_filter.param.yaml
+
     grid_map_type: "OccupancyGridMapFixedBlindSpot"
     OccupancyGridMapFixedBlindSpot:
       distance_margin: 1.0
diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
index 62cfa4bcb5228..b13fd10988712 100644
--- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
+++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
@@ -12,6 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import re
+
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
@@ -26,15 +28,31 @@
 import yaml
 
 
+def replace_pkg_share_path_with_package_name(param_string: str) -> str:
+    # replace $(find-pkg-share ...) with the actual path
+    # get the package name from the string
+    match = re.search(r"\$\(\s*find-pkg-share\s+([a-zA-Z0-9_]+)\s*\)", param_string)
+    if match:
+        package_name = match.group(1)
+        package_path = get_package_share_directory(package_name)
+        # Replace the entire $(find-pkg-share ...) expression with the package path
+        return param_string.replace(match.group(0), package_path)
+    else:
+        # If no match is found, return the original string
+        return param_string
+
+
 def launch_setup(context, *args, **kwargs):
     # load parameter files
     param_file = LaunchConfiguration("param_file").perform(context)
     with open(param_file, "r") as f:
         laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"]
-
-    updater_param_file = LaunchConfiguration("updater_param_file").perform(context)
-    with open(updater_param_file, "r") as f:
-        occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
+        # replace $(find-pkg-share ...) with the actual path
+        laserscan_based_occupancy_grid_map_node_params[
+            "updater_param_file"
+        ] = replace_pkg_share_path_with_package_name(
+            laserscan_based_occupancy_grid_map_node_params["updater_param_file"]
+        )
 
     composable_nodes = [
         ComposableNode(
@@ -101,13 +119,11 @@ def launch_setup(context, *args, **kwargs):
             ],
             parameters=[
                 laserscan_based_occupancy_grid_map_node_params,
-                occupancy_grid_map_updater_params,
                 {
                     "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
                     "input_obstacle_and_raw_pointcloud": LaunchConfiguration(
                         "input_obstacle_and_raw_pointcloud"
                     ),
-                    "updater_type": LaunchConfiguration("updater_type"),
                 },
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -165,12 +181,6 @@ def add_launch_arg(name: str, default_value=None):
                 get_package_share_directory("probabilistic_occupancy_grid_map")
                 + "/config/laserscan_based_occupancy_grid_map.param.yaml",
             ),
-            add_launch_arg("updater_type", "binary_bayes_filter"),
-            add_launch_arg(
-                "updater_param_file",
-                get_package_share_directory("probabilistic_occupancy_grid_map")
-                + "/config/updater.param.yaml",
-            ),
             add_launch_arg("input_obstacle_pointcloud", "false"),
             add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
             add_launch_arg("use_pointcloud_container", "false"),
diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
index 29435c4ea8e24..be7aaae85c0b2 100644
--- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
+++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
@@ -12,6 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import re
+
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
@@ -26,6 +28,20 @@
 import yaml
 
 
+def replace_pkg_share_path_with_package_name(param_string: str) -> str:
+    # replace $(find-pkg-share ...) with the actual path
+    # get the package name from the string
+    match = re.search(r"\$\(\s*find-pkg-share\s+([a-zA-Z0-9_]+)\s*\)", param_string)
+    if match:
+        package_name = match.group(1)
+        package_path = get_package_share_directory(package_name)
+        # Replace the entire $(find-pkg-share ...) expression with the package path
+        return param_string.replace(match.group(0), package_path)
+    else:
+        # If no match is found, return the original string
+        return param_string
+
+
 def launch_setup(context, *args, **kwargs):
     # load parameter files
     param_file = LaunchConfiguration("param_file").perform(context)
@@ -33,10 +49,12 @@ def launch_setup(context, *args, **kwargs):
         pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][
             "ros__parameters"
         ]
-
-    updater_param_file = LaunchConfiguration("updater_param_file").perform(context)
-    with open(updater_param_file, "r") as f:
-        occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
+        # replace $(find-pkg-share ...) with the actual path
+        pointcloud_based_occupancy_grid_map_node_params[
+            "updater_param_file"
+        ] = replace_pkg_share_path_with_package_name(
+            pointcloud_based_occupancy_grid_map_node_params["updater_param_file"]
+        )
 
     composable_nodes = [
         ComposableNode(
@@ -56,8 +74,6 @@ def launch_setup(context, *args, **kwargs):
             ],
             parameters=[
                 pointcloud_based_occupancy_grid_map_node_params,
-                occupancy_grid_map_updater_params,
-                {"updater_type": LaunchConfiguration("updater_type")},
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),
@@ -113,12 +129,6 @@ def add_launch_arg(name: str, default_value=None):
                 get_package_share_directory("probabilistic_occupancy_grid_map")
                 + "/config/pointcloud_based_occupancy_grid_map.param.yaml",
             ),
-            add_launch_arg("updater_type", "binary_bayes_filter"),
-            add_launch_arg(
-                "updater_param_file",
-                get_package_share_directory("probabilistic_occupancy_grid_map")
-                + "/config/binary_bayes_filter_updater.param.yaml",
-            ),
             set_container_executable,
             set_container_mt_executable,
         ]

From 5ab3d14e73b63cce3f7a60a85082fd0b6735f0d4 Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Wed, 7 Feb 2024 11:35:52 +0900
Subject: [PATCH 2/2] feat: fix param loader

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../launch/laserscan_based_occupancy_grid_map.launch.py     | 4 ++++
 .../launch/pointcloud_based_occupancy_grid_map.launch.py    | 6 ++++++
 2 files changed, 10 insertions(+)

diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
index b13fd10988712..c23cb020d5544 100644
--- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
+++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py
@@ -25,6 +25,7 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+import launch_ros.parameter_descriptions
 import yaml
 
 
@@ -119,6 +120,9 @@ def launch_setup(context, *args, **kwargs):
             ],
             parameters=[
                 laserscan_based_occupancy_grid_map_node_params,
+                launch_ros.parameter_descriptions.ParameterFile(
+                    param_file=laserscan_based_occupancy_grid_map_node_params["updater_param_file"],
+                ),
                 {
                     "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
                     "input_obstacle_and_raw_pointcloud": LaunchConfiguration(
diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
index be7aaae85c0b2..d6ead26b0c0ad 100644
--- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
+++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py
@@ -25,6 +25,7 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+import launch_ros.parameter_descriptions
 import yaml
 
 
@@ -74,6 +75,11 @@ def launch_setup(context, *args, **kwargs):
             ],
             parameters=[
                 pointcloud_based_occupancy_grid_map_node_params,
+                launch_ros.parameter_descriptions.ParameterFile(
+                    param_file=pointcloud_based_occupancy_grid_map_node_params[
+                        "updater_param_file"
+                    ],
+                ),
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),