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, ]