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 fce3da96ce888..f99416cc5ac0a 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 @@ -51,6 +51,7 @@ + diff --git a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml new file mode 100644 index 0000000000000..f9a1c4f930001 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -0,0 +1,53 @@ +# sample grid map fusion parameters for sample sensor kit +/**: + ros__parameters: + # shared parameters + shared_config: + map_frame: "map" + base_link_frame: "base_link" + # center of the grid map + gridmap_origin_frame: "base_link" + + map_resolution: 0.5 # [m] + map_length_x: 150.0 # [m] + map_length_y: 150.0 # [m] + + # each grid map parameters + ogm_creation_config: + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: true + # use sensor pointcloud to filter obstacle pointcloud + filter_obstacle_pointcloud_by_raw_pointcloud: true + + grid_map_type: "OccupancyGridMapFixedBlindSpot" + OccupancyGridMapFixedBlindSpot: + distance_margin: 1.0 + OccupancyGridMapProjectiveBlindSpot: + projection_dz_threshold: 0.01 # [m] for avoiding null division + obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length + pub_debug_grid: false + + # parameter settings for ogm fusion + fusion_config: + # following parameters are shared: map_frame, base_link_frame, gridmap_origin_frame, map_resolution, map_length + # Setting1: tune ogm creation parameters + raw_pointcloud_topics: # put each sensor's pointcloud topic + - "/sensing/lidar/top/pointcloud" + - "/sensing/lidar/left/pointcloud" + - "/sensing/lidar/right/pointcloud" + fusion_input_ogm_topics: + - "/perception/occupancy_grid_map/top_lidar/map" + - "/perception/occupancy_grid_map/left_lidar/map" + - "/perception/occupancy_grid_map/right_lidar/map" + # reliability of each sensor (0.0 ~ 1.0) only work with "log-odds" and "dempster-shafer" + input_ogm_reliabilities: + - 1.0 + - 0.6 + - 0.6 + + # Setting2: tune ogm fusion parameters + ## choose fusion method from ["overwrite", "log-odds", "dempster-shafer"] + fusion_method: "overwrite" diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..29f8ed5985d48 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -0,0 +1,211 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + +# In this file, we use "ogm" as a meaning of occupancy grid map + + +# overwrite parameter +def overwrite_config(param_dict, launch_config_name, node_params_name, context): + if LaunchConfiguration(launch_config_name).perform(context) != "": + param_dict[node_params_name] = LaunchConfiguration(launch_config_name).perform(context) + + +# load parameter files +def load_config_file(context, configuration_name: str): + config_file = LaunchConfiguration(configuration_name).perform(context) + with open(config_file, "r") as f: + config_param_dict = yaml.safe_load(f)["/**"]["ros__parameters"] + return config_param_dict + + +# check if the length of the parameters are the same +def fusion_config_sanity_check(fusion_config: dict): + listed_param_names = [ + "raw_pointcloud_topics", + "fusion_input_ogm_topics", + # "each_ogm_sensor_frames", + "input_ogm_reliabilities", + ] + param_length_list = [] + + for param_name in listed_param_names: + # parameters is not in the config file dict + if param_name not in fusion_config: + raise Exception("Error: " + param_name + " is not in the config file") + # get len of the parameter + param_length_list.append(len(fusion_config[param_name])) + + # check if the length of the parameters are the same + if not all(x == param_length_list[0] for x in param_length_list): + raise Exception("Error: the length of the parameters are not the same") + + +def get_fusion_config(total_config: dict) -> dict: + fusion_config = total_config["fusion_config"] + shared_config = total_config["shared_config"] + # merge shared config and fusion config + fusion_config.update(shared_config) + # overwrite ogm size settings + fusion_config["fusion_map_length_x"] = shared_config["map_length_x"] + fusion_config["fusion_map_length_y"] = shared_config["map_length_y"] + fusion_config["fusion_map_resolution"] = shared_config["map_resolution"] + return fusion_config + + +# extract ogm creation config from fusion config +def get_ogm_creation_config(total_config: dict, list_iter: int) -> dict: + ogm_creation_config = total_config["ogm_creation_config"] + shared_config = total_config["shared_config"] + # fusion_config_ = total_config["fusion_config"] + # merge shared config and ogm creation config + ogm_creation_config.update(shared_config) + # overwrite scan_origin_frame with sensor frame settings + # ogm_creation_config["scan_origin_frame"] = fusion_config_["each_ogm_sensor_frames"][list_iter] + # use fusion_map_length to set map_length + ogm_creation_config["map_length"] = max( + shared_config["map_length_x"], shared_config["map_length_y"] + ) + return ogm_creation_config + + +def launch_setup(context, *args, **kwargs): + """Launch fusion based occupancy grid map creation nodes. + + 1. describe occupancy grid map generation nodes for each sensor input + 2. describe occupancy grid map fusion node + 3. launch setting + """ + # 1. launch occupancy grid map generation nodes for each sensor input + + # load fusion config parameter + total_config = load_config_file(context, "multi_lidar_fusion_config_file") + fusion_config = get_fusion_config(total_config) + fusion_config_sanity_check(fusion_config) + updater_config = load_config_file(context, "updater_param_file") + + # pointcloud based occupancy grid map nodes + gridmap_generation_composable_nodes = [] + + number_of_nodes = len(fusion_config["raw_pointcloud_topics"]) + print(number_of_nodes) + + for i in range(number_of_nodes): + # load parameter file + ogm_creation_config = get_ogm_creation_config(total_config, i) + ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context) + ogm_creation_config.update(updater_config) + + # generate composable node + node = ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + name="occupancy_grid_map_node_in_" + str(i), + remappings=[ + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]), + ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]), + ], + parameters=[ogm_creation_config], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + gridmap_generation_composable_nodes.append(node) + + # 2. launch occupancy grid map fusion node + gridmap_fusion_node = [ + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="synchronized_grid_map_fusion::GridMapFusionNode", + name="occupancy_grid_map_fusion_node", + remappings=[ + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[fusion_config], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + # 3. launch setting + occupancy_grid_map_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return LaunchDescription( + [ + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("container_name", "occupancy_grid_map_container"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + add_launch_arg( + "multi_lidar_fusion_config_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/multi_lidar_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, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 28fe4adafb482..f191a23eb8e65 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -311,10 +311,16 @@ void GridMapFusionNode::publish() if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds())) + .count(); debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index a8cdff5b27907..96afb0f2e852c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -58,7 +58,7 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( map_frame_ = this->declare_parameter("map_frame"); base_link_frame_ = this->declare_parameter("base_link_frame"); gridmap_origin_frame_ = this->declare_parameter("gridmap_origin_frame"); - scan_origin_frame_ = this->declare_parameter("scan_origin_frame"); + scan_origin_frame_ = this->declare_parameter("scan_origin_frame", ""); use_height_filter_ = this->declare_parameter("height_filter.use_height_filter"); min_height_ = this->declare_parameter("height_filter.min_height"); max_height_ = this->declare_parameter("height_filter.max_height"); @@ -136,6 +136,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } + // if scan_origin_frame_ is "", replace it with input_raw_msg->header.frame_id + if (scan_origin_frame_.empty()) { + scan_origin_frame_ = input_raw_msg->header.frame_id; + } + // Apply height filter PointCloud2 cropped_obstacle_pc{}; PointCloud2 cropped_raw_pc{};