chore(probabilistic_occupancy_grid_map): move updater parameter to yaml file

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>

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>
 

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>
 

perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml

@@ -6,17 +6,14 @@
   <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">
     <remap from="~/input/laserscan" to="$(var input/laserscan)"/>
     <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">

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

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

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
@@ -23,91 +25,109 @@
 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
 
 
+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(
             package="pointcloud_to_laserscan",
             plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode",
             name="pointcloud_to_laserscan_node",
             remappings=[
                 (
                     "~/input/pointcloud",
                     LaunchConfiguration("input/obstacle_pointcloud"),
                 ),
                 (
                     "~/output/laserscan",
                     LaunchConfiguration("output/laserscan"),
                 ),
                 (
                     "~/output/pointcloud",
                     LaunchConfiguration("output/pointcloud"),
                 ),
                 ("~/output/ray", LaunchConfiguration("output/ray")),
                 ("~/output/stixel", LaunchConfiguration("output/stixel")),
             ],
             parameters=[
                 {
                     "target_frame": laserscan_based_occupancy_grid_map_node_params[
                         "scan_origin_frame"
                     ],  # Leave disabled to output scan in pointcloud frame
                     "transform_tolerance": 0.01,
                     "min_height": 0.0,
                     "max_height": 2.0,
                     "angle_min": -3.141592,  # -M_PI
                     "angle_max": 3.141592,  # M_PI
                     "angle_increment": 0.00349065850,  # 0.20*M_PI/180.0
                     "scan_time": 0.0,
                     "range_min": 1.0,
                     "range_max": 200.0,
                     "use_inf": True,
                     "inf_epsilon": 1.0,
                     # Concurrency level, affects number of pointclouds queued for processing
                     # and number of threads used
                     # 0 : Detect number of cores
                     # 1 : Single threaded
                     # 2->inf : Parallelism level
                     "concurrency_level": 1,
                 }
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),
         ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode",
             name="occupancy_grid_map_node",
             remappings=[
                 ("~/input/laserscan", LaunchConfiguration("output/laserscan")),
                 (
                     "~/input/obstacle_pointcloud",
                     LaunchConfiguration("input/obstacle_pointcloud"),
                 ),
                 (
                     "~/input/raw_pointcloud",
                     LaunchConfiguration("input/raw_pointcloud"),
                 ),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
             parameters=[
                 laserscan_based_occupancy_grid_map_node_params,
-                occupancy_grid_map_updater_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(
                         "input_obstacle_and_raw_pointcloud"
                     ),
-                    "updater_type": LaunchConfiguration("updater_type"),
                 },
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -165,12 +185,6 @@
                 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"),

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
@@ -23,20 +25,37 @@
 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
 
 
+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:
         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 +75,11 @@ def launch_setup(context, *args, **kwargs):
             ],
             parameters=[
                 pointcloud_based_occupancy_grid_map_node_params,
-                occupancy_grid_map_updater_params,
-                {"updater_type": LaunchConfiguration("updater_type")},
+                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")}],
         ),
@@ -113,12 +135,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,
         ]