feat(probabilistic_occupancy_grid_map): add synchronized ogm fusion n…

…ode (#5485)

* add synchronized ogm fusion node

Signed-off-by: yoshiri <[email protected]>

* add launch test for grid map fusion node

Signed-off-by: yoshiri <[email protected]>

* fix test cases input msg error

Signed-off-by: yoshiri <[email protected]>

* change default fusion parameter

Signed-off-by: yoshiri <[email protected]>

* rename parameter for ogm fusion

Signed-off-by: yoshiri <[email protected]>

* feat: add multi_lidar_ogm generation method

Signed-off-by: yoshiri <[email protected]>

* enable ogm creation launcher in tier4_perception_launch to call multi_lidar ogm creation

Signed-off-by: yoshiri <[email protected]>

* fix: change ogm fusion node pub policy to reliable

Signed-off-by: yoshiri <[email protected]>

* fix: fix to use lidar frame as scan frame

Signed-off-by: Yoshi, Ri <[email protected]>

* fix: launcher node

Signed-off-by: Yoshi, Ri <[email protected]>

* feat: update param name

Signed-off-by: yoshiri <[email protected]>

* chore: fix ogm pointcloud subscription

Signed-off-by: yoshiri <[email protected]>

* feat: enable to publish pipeline latency

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
Signed-off-by: Yoshi, Ri <[email protected]>

launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml

@@ -51,6 +51,7 @@
   <!--multi lidar pointclouds based method-->
   <group if="$(eval &quot;'$(var occupancy_grid_map_method)'=='multi_lidar_pointcloud_based_occupancy_grid_map'&quot;)">
     <include file="$(find-pkg-share probabilistic_occupancy_grid_map)/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py">
+      <arg name="input/obstacle_pointcloud" value="$(var input/obstacle_pointcloud)"/>
       <arg name="output" value="/perception/occupancy_grid_map/map"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>

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"

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)]
+    )

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<double, std::milli>(
+        std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds()))
+        .count();
     debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/cyclic_time_ms", cyclic_time_ms);
     debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/processing_time_ms", processing_time_ms);
+    debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/pipeline_latency_ms", pipeline_latency_ms);
   }
 }
 

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<std::string>("map_frame");
   base_link_frame_ = this->declare_parameter<std::string>("base_link_frame");
   gridmap_origin_frame_ = this->declare_parameter<std::string>("gridmap_origin_frame");
-  scan_origin_frame_ = this->declare_parameter<std::string>("scan_origin_frame");
+  scan_origin_frame_ = this->declare_parameter<std::string>("scan_origin_frame", "");
   use_height_filter_ = this->declare_parameter<bool>("height_filter.use_height_filter");
   min_height_ = this->declare_parameter<double>("height_filter.min_height");
   max_height_ = this->declare_parameter<double>("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{};