feat(probablistic_occupancy_grid_map): add grid map fusion node to fuse multiple sensor based occupancy grid map

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

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<launch>
+  <!--default parameter follows node setting -->
+  <arg name="input/obstacle_pointcloud" default="concatenated/pointcloud"/>
+  <arg name="input/raw_pointcloud" default="no_ground/oneshot/pointcloud"/>
+  <arg name="output" default="/perception/occupancy_grid_map/map"/>
+  <arg name="use_intra_process" default="false"/>
+  <arg name="use_multithread" default="false"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="container_name" default="occupancy_grid_map_container"/>
+  <arg name="scan_origin" default="base_link"/>
+  <arg name="map_origin" default="base_link"/>
+
+  <!--default parameter follows node setting -->
+  <arg name="method" default="pointcloud_based_occupancy_grid_map"/>
+  <arg name="param_file" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/$(var method).param.yaml"/>
+
+  <!--pointcloud based method-->
+  <group if="$(eval &quot;'$(var method)'=='pointcloud_based_occupancy_grid_map'&quot;)">
+    <include file="$(find-pkg-share probabilistic_occupancy_grid_map)/launch/pointcloud_based_occupancy_grid_map.launch.py">
+      <arg name="input/obstacle_pointcloud" value="$(var input/obstacle_pointcloud)"/>
+      <arg name="input/raw_pointcloud" value="$(var input/raw_pointcloud)"/>
+      <arg name="output" value="$(var output)"/>
+      <arg name="use_intra_process" value="$(var use_intra_process)"/>
+      <arg name="use_multithread" value="$(var use_multithread)"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="container_name" value="$(var container_name)"/>
+      <arg name="param_file" value="$(var param_file)"/>
+      <arg name="scan_origin" value="$(var scan_origin)"/>
+      <arg name="map_origin" value="$(var map_origin)"/>
+    </include>
+  </group>
+
+  <!--laserscan based method-->
+  <group if="$(eval &quot;'$(var method)'=='laserscan_based_occupancy_grid_map'&quot;)">
+    <include file="$(find-pkg-share probabilistic_occupancy_grid_map)/launch/laserscan_based_occupancy_grid_map.launch.py">
+      <arg name="input/obstacle_pointcloud" value="$(var input/obstacle_pointcloud)"/>
+      <arg name="input/raw_pointcloud" value="$(var input/raw_pointcloud)"/>
+      <arg name="output" value="$(var output)"/>
+      <arg name="use_intra_process" value="$(var use_intra_process)"/>
+      <arg name="use_multithread" value="$(var use_multithread)"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="container_name" value="$(var container_name)"/>
+      <arg name="param_file" value="$(var param_file)"/>
+      <arg name="scan_origin" value="$(var scan_origin)"/>
+      <arg name="map_origin" value="$(var map_origin)"/>
+    </include>
+  </group>
+</launch>

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -68,14 +68,15 @@
     <!-- occupancy grid map module -->
     <group>
       <push-ros-namespace namespace="occupancy_grid_map"/>
-      <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py">
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud_raw"/>
         <arg name="input/raw_pointcloud" value="$(var input/pointcloud)"/>
         <arg name="output" value="/perception/occupancy_grid_map/map"/>
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="method" value="pointcloud_based_occupancy_grid_map"/>
         <arg name="param_file" value="$(find-pkg-share probabilistic_occupancy_grid_map)/config/pointcloud_based_occupancy_grid_map.param.yaml"/>
       </include>
     </group>

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -39,12 +39,14 @@
   <group unless="$(var scenario_simulation)">
     <!-- Occupancy Grid -->
     <push-ros-namespace namespace="occupancy_grid_map"/>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
       <arg name="input_obstacle_pointcloud" value="true"/>
       <arg name="input_obstacle_and_raw_pointcloud" value="false"/>
       <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
       <arg name="output" value="/perception/occupancy_grid_map/map"/>
-      <arg name="param_file" value="$(find-pkg-share probabilistic_occupancy_grid_map)/config/laserscan_based_occupancy_grid_map.param.yaml"/>
+      <arg name="method" value="laserscan_based_occupancy_grid_map"/>
+      <arg name="map_origin" value="base_link"/>
+      <arg name="scan_origin" value="base_link"/>
     </include>
   </group>
 

perception/probabilistic_occupancy_grid_map/CMakeLists.txt

@@ -47,6 +47,36 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map
   EXECUTABLE laserscan_based_occupancy_grid_map_node
 )
 
+# GridMapFusionNode
+ament_auto_add_library(grid_map_fusion SHARED
+  src/fusion/grid_map_fusion_node.cpp
+  src/fusion/single_frame_fusion_policy.cpp
+  src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp
+  src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp
+  src/utils/utils.cpp
+)
+
+target_link_libraries(grid_map_fusion
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(grid_map_fusion
+  PLUGIN "grid_map_fusion::GridMapFusionNode"
+  EXECUTABLE grid_map_fusion_node
+)
+
+# Tests
+if(BUILD_TESTING)
+  ament_add_gtest(costmap_unit_tests
+    test/cost_value_test.cpp)
+  ament_add_gtest(fusion_policy_unit_tests
+    test/fusion_policy_test.cpp
+    src/fusion/single_frame_fusion_policy.cpp
+  )
+  target_include_directories(costmap_unit_tests PRIVATE "include")
+  target_include_directories(fusion_policy_unit_tests PRIVATE "include")
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch

perception/probabilistic_occupancy_grid_map/config/fusion_map.param.yaml

@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    map_length: 100.0     # [m]
+    map_resolution: 0.5 # [m]
+
+    use_height_filter: true
+    enable_single_frame_mode: true
+    # use sensor pointcloud to filter obstacle pointcloud
+    filter_obstacle_pointcloud_by_raw_pointcloud: true
+
+    # grid map coordinate
+    map_frame: "map"
+    base_link_frame: "base_link"
+    # center of the grid map
+    gridmap_origin_frame: "base_link"

perception/probabilistic_occupancy_grid_map/config/grid_map_fusion.param.yaml

@@ -0,0 +1,36 @@
+# sample grid map fusion parameters for sample sensor kit
+/**:
+    ros__parameters:
+      fusion_map_length_x: 100.0     # [m]
+      fusion_map_length_y: 100.0     # [m]
+      fusion_map_resolution: 0.5 # [m]
+
+      # Setting1: tune ogm creation parameters
+      obstacle_pointcloud_topic: "/perception/obstacle_segmentation/single_frame/pointcloud_raw"
+      each_raw_pointcloud_topics:
+        - "/sensing/lidar/top/outlier_filtered/pointcloud_synchronized"
+        - "/sensing/lidar/left/outlier_filtered/pointcloud_synchronized"
+        - "/sensing/lidar/right/outlier_filtered/pointcloud_synchronized"
+      each_ogm_output_topics:
+        - "/perception/occupancy_grid_map/top_lidar/map"
+        - "/perception/occupancy_grid_map/left_lidar/map"
+        - "/perception/occupancy_grid_map/right_lidar/map"
+      each_ogm_sensor_frames:
+        - "velodyne_top"
+        - "velodyne_left"
+        - "velodyne_right"
+      # reliability of each sensor (0.0 ~ 1.0) only work with "log-odds" and "dempster-shafer"
+      each_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"
+
+      # grid map coordinate
+      map_frame: "map"
+      base_link_frame: "base_link"
+      # center of the grid map
+      gridmap_origin_frame: "base_link"

perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    map_length: 100.0     # [m]
+    map_resolution: 0.5 # [m]
+
+    use_height_filter: true
+    enable_single_frame_mode: true
+    # use sensor pointcloud to filter obstacle pointcloud
+    filter_obstacle_pointcloud_by_raw_pointcloud: true
+
+    # grid map coordinate
+    map_frame: "map"
+    base_link_frame: "base_link"
+    # center of the grid map
+    gridmap_origin_frame: "base_link"
+    # ray-tracing center: main sensor frame is preferable like: "velodyne_top"
+    scan_origin_frame: "base_link"