feat(probabilistic_occupancy_grid_map): add grid map fusion node (aut…

…owarefoundation#5993) * add synchronized ogm fusion node Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * add launch test for grid map fusion node Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix test cases input msg error Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * change default fusion parameter Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * rename parameter for ogm fusion Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add multi_lidar_ogm generation method Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * enable ogm creation launcher in tier4_perception_launch to call multi_lidar ogm creation Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: change ogm fusion node pub policy to reliable Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * chore: remove files outof scope with divied PR Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
Interplai · May 28, 2024 · 9e1005b · 9e1005b
1 parent 216f601
commit 9e1005b
Show file tree

Hide file tree

Showing 19 changed files with 2,064 additions and 3 deletions.
diff --git a/...4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/...4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml
@@ -8,7 +8,7 @@
   <arg name="use_multithread" default="false"/>
   <arg name="use_pointcloud_container" default="false"/>
   <arg name="container_name" default="occupancy_grid_map_container"/>
-  <arg name="occupancy_grid_map_method" description="options: pointcloud_based_occupancy_grid_map, laserscan_based_occupancy_grid_map"/>
+  <arg name="occupancy_grid_map_method" description="options: pointcloud_based_occupancy_grid_map, laserscan_based_occupancy_grid_map, multi_lidar_pointcloud_based_occupancy_grid_map"/>
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater" description="options: binary_bayes_filter"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
@@ -48,4 +48,18 @@
       <arg name="input_obstacle_and_raw_pointcloud" value="$(var input_obstacle_and_raw_pointcloud)"/>
     </include>
   </group>
+
+  <!--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="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="multi_lidar_fusion_config_file" value="$(var occupancy_grid_map_param_path)"/>
+      <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
+      <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
+    </include>
+  </group>
 </launch>
diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt
@@ -55,6 +55,24 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map
   EXECUTABLE laserscan_based_occupancy_grid_map_node
 )
 
+# GridMapFusionNode
+ament_auto_add_library(synchronized_grid_map_fusion SHARED
+  src/fusion/synchronized_grid_map_fusion_node.cpp
+  src/fusion/single_frame_fusion_policy.cpp
+  src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp
+  src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp
+  src/utils/utils.cpp
+)
+
+target_link_libraries(synchronized_grid_map_fusion
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(synchronized_grid_map_fusion
+  PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode"
+  EXECUTABLE synchronized_grid_map_fusion_node
+)
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
@@ -66,13 +84,22 @@ if(BUILD_TESTING)
   # launch_testing
   find_package(launch_testing_ament_cmake REQUIRED)
   add_launch_test(test/test_pointcloud_based_method.py)
+  add_launch_test(test/test_synchronized_grid_map_fusion_node.py)
 
   # gtest
   ament_add_gtest(test_utils
   test/test_utils.cpp
   )
+  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_link_libraries(test_utils
     ${PCL_LIBRARIES}
     ${PROJECT_NAME}_common
   )
+  target_include_directories(costmap_unit_tests PRIVATE "include")
+  target_include_directories(fusion_policy_unit_tests PRIVATE "include")
 endif()
diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md
@@ -9,6 +9,7 @@ This package outputs the probability of having an obstacle as occupancy grid map
 
 - [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md)
 - [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md)
+- [Grid map fusion](synchronized_grid_map_fusion.md)
 
 ## Settings
 
@@ -70,3 +71,19 @@ Additional argument is shown below:
 | `container_name`                    | `occupancy_grid_map_container` |                                                                                               |
 | `input_obstacle_pointcloud`         | `false`                        | only for laserscan based method. If true, the node subscribe obstacle pointcloud              |
 | `input_obstacle_and_raw_pointcloud` | `true`                         | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud |
+
+### Test
+
+This package provides unit tests using `gtest`.
+You can run the test by the following command.
+
+```bash
+colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+
+```
+
+Test contains the following.
+
+- Unit test for cost value conversion function
+- Unit test for utility functions
+- Unit test for occupancy grid map fusion functions
+- Input/Output test for pointcloud based occupancy grid map
diff --git a/...tion/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml b/...tion/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml
@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+    # 1. fusion parameters
+    fusion_input_ogm_topics: ["topic1", "topic2"]
+    input_ogm_reliabilities: [0.8, 0.2]
+    fusion_method: "overwrite" # choose from ["overwrite", "log-odds", "dempster-shafer"]
+
+    # 2. synchronization settings
+    match_threshold_sec: 0.01    # 10ms
+    timeout_sec: 0.1             # 100ms
+    input_offset_sec: [0.0, 0.0] # no offset
+
+    # 3. settings for fused fusion map
+    # remember resolution and map size should be same with input maps
+    map_frame_: "map"
+    base_link_frame_: "base_link"
+    grid_map_origin_frame_: "base_link"
+    fusion_map_length_x: 100.0
+    fusion_map_length_y: 100.0
+    fusion_map_resolution: 0.5