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 @@
-
-
@@ -26,8 +24,6 @@
-
-
@@ -42,8 +38,6 @@
-
-
@@ -58,8 +52,6 @@
-
-
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 @@
-
-
@@ -150,8 +148,6 @@
-
-
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 @@
-
-
@@ -51,8 +49,6 @@
-
-
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 @@
-
@@ -14,9 +13,7 @@
-
-
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..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
@@ -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,18 +25,35 @@
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(
@@ -101,13 +120,14 @@ def launch_setup(context, *args, **kwargs):
],
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 @@ 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..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
@@ -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,9 +25,24 @@
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)
@@ -33,10 +50,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 +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,
]