|
| 1 | +# Copyright 2021 Tier IV, Inc. All rights reserved. |
| 2 | +# |
| 3 | +# Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +# you may not use this file except in compliance with the License. |
| 5 | +# You may obtain a copy of the License at |
| 6 | +# |
| 7 | +# http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +# |
| 9 | +# Unless required by applicable law or agreed to in writing, software |
| 10 | +# distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +# See the License for the specific language governing permissions and |
| 13 | +# limitations under the License. |
| 14 | + |
| 15 | + |
| 16 | +from ament_index_python.packages import get_package_share_directory |
| 17 | +from launch import LaunchDescription |
| 18 | +from launch.actions import DeclareLaunchArgument |
| 19 | +from launch.actions import OpaqueFunction |
| 20 | +from launch.actions import SetLaunchConfiguration |
| 21 | +from launch.conditions import IfCondition |
| 22 | +from launch.conditions import UnlessCondition |
| 23 | +from launch.substitutions import LaunchConfiguration |
| 24 | +from launch_ros.actions import ComposableNodeContainer |
| 25 | +from launch_ros.actions import LoadComposableNodes |
| 26 | +from launch_ros.descriptions import ComposableNode |
| 27 | +import yaml |
| 28 | + |
| 29 | +# In this file, we use "ogm" as a meaning of occupancy grid map |
| 30 | + |
| 31 | + |
| 32 | +# overwrite parameter |
| 33 | +def overwrite_config(param_dict, launch_config_name, node_params_name, context): |
| 34 | + if LaunchConfiguration(launch_config_name).perform(context) != "": |
| 35 | + param_dict[node_params_name] = LaunchConfiguration(launch_config_name).perform(context) |
| 36 | + |
| 37 | + |
| 38 | +# load parameter files |
| 39 | +def load_config_file(context, configuration_name: str): |
| 40 | + config_file = LaunchConfiguration(configuration_name).perform(context) |
| 41 | + with open(config_file, "r") as f: |
| 42 | + config_param_dict = yaml.safe_load(f)["/**"]["ros__parameters"] |
| 43 | + return config_param_dict |
| 44 | + |
| 45 | + |
| 46 | +# check if the length of the parameters are the same |
| 47 | +def fusion_config_sanity_check(fusion_config: dict): |
| 48 | + listed_param_names = [ |
| 49 | + "raw_pointcloud_topics", |
| 50 | + "fusion_input_ogm_topics", |
| 51 | + # "each_ogm_sensor_frames", |
| 52 | + "input_ogm_reliabilities", |
| 53 | + ] |
| 54 | + param_length_list = [] |
| 55 | + |
| 56 | + for param_name in listed_param_names: |
| 57 | + # parameters is not in the config file dict |
| 58 | + if param_name not in fusion_config: |
| 59 | + raise Exception("Error: " + param_name + " is not in the config file") |
| 60 | + # get len of the parameter |
| 61 | + param_length_list.append(len(fusion_config[param_name])) |
| 62 | + |
| 63 | + # check if the length of the parameters are the same |
| 64 | + if not all(x == param_length_list[0] for x in param_length_list): |
| 65 | + raise Exception("Error: the length of the parameters are not the same") |
| 66 | + |
| 67 | + |
| 68 | +def get_fusion_config(total_config: dict) -> dict: |
| 69 | + fusion_config = total_config["fusion_config"] |
| 70 | + shared_config = total_config["shared_config"] |
| 71 | + # merge shared config and fusion config |
| 72 | + fusion_config.update(shared_config) |
| 73 | + # overwrite ogm size settings |
| 74 | + fusion_config["fusion_map_length_x"] = shared_config["map_length_x"] |
| 75 | + fusion_config["fusion_map_length_y"] = shared_config["map_length_y"] |
| 76 | + fusion_config["fusion_map_resolution"] = shared_config["map_resolution"] |
| 77 | + return fusion_config |
| 78 | + |
| 79 | + |
| 80 | +# extract ogm creation config from fusion config |
| 81 | +def get_ogm_creation_config(total_config: dict, list_iter: int) -> dict: |
| 82 | + ogm_creation_config = total_config["ogm_creation_config"] |
| 83 | + shared_config = total_config["shared_config"] |
| 84 | + # fusion_config_ = total_config["fusion_config"] |
| 85 | + # merge shared config and ogm creation config |
| 86 | + ogm_creation_config.update(shared_config) |
| 87 | + # overwrite scan_origin_frame with sensor frame settings |
| 88 | + # ogm_creation_config["scan_origin_frame"] = fusion_config_["each_ogm_sensor_frames"][list_iter] |
| 89 | + # use fusion_map_length to set map_length |
| 90 | + ogm_creation_config["map_length"] = max( |
| 91 | + shared_config["map_length_x"], shared_config["map_length_y"] |
| 92 | + ) |
| 93 | + return ogm_creation_config |
| 94 | + |
| 95 | + |
| 96 | +def launch_setup(context, *args, **kwargs): |
| 97 | + """Launch fusion based occupancy grid map creation nodes. |
| 98 | +
|
| 99 | + 1. describe occupancy grid map generation nodes for each sensor input |
| 100 | + 2. describe occupancy grid map fusion node |
| 101 | + 3. launch setting |
| 102 | + """ |
| 103 | + # 1. launch occupancy grid map generation nodes for each sensor input |
| 104 | + |
| 105 | + # load fusion config parameter |
| 106 | + total_config = load_config_file(context, "multi_lidar_fusion_config_file") |
| 107 | + fusion_config = get_fusion_config(total_config) |
| 108 | + fusion_config_sanity_check(fusion_config) |
| 109 | + updater_config = load_config_file(context, "updater_param_file") |
| 110 | + |
| 111 | + # pointcloud based occupancy grid map nodes |
| 112 | + gridmap_generation_composable_nodes = [] |
| 113 | + |
| 114 | + number_of_nodes = len(fusion_config["raw_pointcloud_topics"]) |
| 115 | + print(number_of_nodes) |
| 116 | + |
| 117 | + for i in range(number_of_nodes): |
| 118 | + # load parameter file |
| 119 | + ogm_creation_config = get_ogm_creation_config(total_config, i) |
| 120 | + ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context) |
| 121 | + ogm_creation_config.update(updater_config) |
| 122 | + |
| 123 | + # generate composable node |
| 124 | + node = ComposableNode( |
| 125 | + package="probabilistic_occupancy_grid_map", |
| 126 | + plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", |
| 127 | + name="occupancy_grid_map_node_in_" + str(i), |
| 128 | + remappings=[ |
| 129 | + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), |
| 130 | + ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]), |
| 131 | + ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]), |
| 132 | + ], |
| 133 | + parameters=[ogm_creation_config], |
| 134 | + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], |
| 135 | + ) |
| 136 | + gridmap_generation_composable_nodes.append(node) |
| 137 | + |
| 138 | + # 2. launch occupancy grid map fusion node |
| 139 | + gridmap_fusion_node = [ |
| 140 | + ComposableNode( |
| 141 | + package="probabilistic_occupancy_grid_map", |
| 142 | + plugin="synchronized_grid_map_fusion::GridMapFusionNode", |
| 143 | + name="occupancy_grid_map_fusion_node", |
| 144 | + remappings=[ |
| 145 | + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), |
| 146 | + ], |
| 147 | + parameters=[fusion_config], |
| 148 | + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], |
| 149 | + ), |
| 150 | + ] |
| 151 | + |
| 152 | + # 3. launch setting |
| 153 | + occupancy_grid_map_container = ComposableNodeContainer( |
| 154 | + name=LaunchConfiguration("container_name"), |
| 155 | + namespace="", |
| 156 | + package="rclcpp_components", |
| 157 | + executable=LaunchConfiguration("container_executable"), |
| 158 | + composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node, |
| 159 | + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), |
| 160 | + output="screen", |
| 161 | + ) |
| 162 | + |
| 163 | + load_composable_nodes = LoadComposableNodes( |
| 164 | + composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node, |
| 165 | + target_container=LaunchConfiguration("container_name"), |
| 166 | + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), |
| 167 | + ) |
| 168 | + |
| 169 | + return [occupancy_grid_map_container, load_composable_nodes] |
| 170 | + |
| 171 | + |
| 172 | +def generate_launch_description(): |
| 173 | + def add_launch_arg(name: str, default_value=None): |
| 174 | + return DeclareLaunchArgument(name, default_value=default_value) |
| 175 | + |
| 176 | + set_container_executable = SetLaunchConfiguration( |
| 177 | + "container_executable", |
| 178 | + "component_container", |
| 179 | + condition=UnlessCondition(LaunchConfiguration("use_multithread")), |
| 180 | + ) |
| 181 | + |
| 182 | + set_container_mt_executable = SetLaunchConfiguration( |
| 183 | + "container_executable", |
| 184 | + "component_container_mt", |
| 185 | + condition=IfCondition(LaunchConfiguration("use_multithread")), |
| 186 | + ) |
| 187 | + |
| 188 | + return LaunchDescription( |
| 189 | + [ |
| 190 | + add_launch_arg("use_multithread", "false"), |
| 191 | + add_launch_arg("use_intra_process", "true"), |
| 192 | + add_launch_arg("use_pointcloud_container", "false"), |
| 193 | + add_launch_arg("container_name", "occupancy_grid_map_container"), |
| 194 | + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), |
| 195 | + add_launch_arg("output", "occupancy_grid"), |
| 196 | + add_launch_arg( |
| 197 | + "multi_lidar_fusion_config_file", |
| 198 | + get_package_share_directory("probabilistic_occupancy_grid_map") |
| 199 | + + "/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml", |
| 200 | + ), |
| 201 | + add_launch_arg("updater_type", "binary_bayes_filter"), |
| 202 | + add_launch_arg( |
| 203 | + "updater_param_file", |
| 204 | + get_package_share_directory("probabilistic_occupancy_grid_map") |
| 205 | + + "/config/binary_bayes_filter_updater.param.yaml", |
| 206 | + ), |
| 207 | + set_container_executable, |
| 208 | + set_container_mt_executable, |
| 209 | + ] |
| 210 | + + [OpaqueFunction(function=launch_setup)] |
| 211 | + ) |
0 commit comments