feat: pointcloud based probabilistic occupancy grid map (#624)

* initial commit

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* ci(pre-commit): autofix

* change param

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* update readme

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add launch

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* ci(pre-commit): autofix

* update readme

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* ci(pre-commit): autofix

* fix typo

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* update readme

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* ci(pre-commit): autofix

* cosmetic change

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add single frame mode

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py

@@ -76,8 +76,8 @@ def add_launch_arg(name: str, default_value=None):
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),
         ComposableNode(
-            package="laserscan_to_occupancy_grid_map",
-            plugin="occupancy_grid_map::OccupancyGridMapNode",
+            package="probabilistic_occupancy_grid_map",
+            plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode",
             name="occupancy_grid_map_node",
             remappings=[
                 ("~/input/laserscan", LaunchConfiguration("output/laserscan")),

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -51,11 +51,11 @@
     <group>
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <!-- Occupancy Grid -->
-      <include file="$(find-pkg-share laserscan_to_occupancy_grid_map)/launch/laserscan_to_occupancy_grid_map.launch.py">
-        <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" />
+      <include file="$(find-pkg-share probabilistic_occupancy_grid_map)/launch/laserscan_based_occupancy_grid_map.launch.py">
+        <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"/>
       </include>
     </group>
 

perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="input/detected_objects" default="/perception/object_recognition/detection/objects" />
+  <arg name="output/objects" default="/perception/object_recognition/detection/validation/occupancy_grid_based/objects" />
+
+  <arg name="input/laserscan" default="/perception/occupancy_grid_map/virtual_scan/laserscan" />
+  <arg name="input/occupancy_grid_map" default="/perception/object_recognition/detection/validation/occupancy_grid_based/single_frame_occupancy_grid_map" />
+
+
+  <node pkg="probabilistic_occupancy_grid_map" exec="laserscan_based_occupancy_grid_map_node" name="single_frame_laserscan_occupancy_grid_map" output="screen">
+    <remap from="~/input/laserscan" to="$(var input/laserscan)"/>
+    <remap from="~/output/occupancy_grid_map" to="$(var input/occupancy_grid_map)"/>
+    <param name="input_obstacle_pointcloud" value="false"/>
+    <param name="input_obstacle_and_raw_pointcloud" value="false"/>
+    <param name="map_resolution" value="0.2"/>
+    <param name="map_length" value="200.0"/>
+    <param name="map_width" value="20.0"/>
+    <param name="map_frame" value="base_link"/>
+    <param name="enable_single_frame_mode" value="true"/>
+  </node>
+
+  <node pkg="detected_object_validation" exec="occupancy_grid_based_validator_node" name="occupancy_grid_based_validator_node" output="screen">
+    <remap from="~/input/detected_objects" to="$(var input/detected_objects)" />
+    <remap from="~/input/occupancy_grid_map" to="$(var input/occupancy_grid_map)" />
+    <remap from="~/output/objects" to="$(var output/objects)" />
+    <param name="map_frame" value="base_link" />
+    <param name="enable_debug" value="false" />
+  </node>
+
+</launch>

sensing/probabilistic_occupancy_grid_map/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(probabilistic_occupancy_grid_map)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
+ament_auto_find_build_dependencies()
+
+# PointcloudBasedOccupancyGridMap
+ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED
+  src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
+  src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp
+  src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp
+)
+
+target_link_libraries(pointcloud_based_occupancy_grid_map
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(pointcloud_based_occupancy_grid_map
+  PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode"
+  EXECUTABLE pointcloud_based_occupancy_grid_map_node
+)
+
+# LaserscanBasedOccupancyGridMap
+ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED
+  src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp
+  src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp
+  src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp
+)
+
+target_link_libraries(laserscan_based_occupancy_grid_map
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(laserscan_based_occupancy_grid_map
+  PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode"
+  EXECUTABLE laserscan_based_occupancy_grid_map_node
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    # To avoid conflicts between cpplint and uncrustify w.r.t. inclusion guards
+    ament_cmake_uncrustify
+  )
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)

sensing/probabilistic_occupancy_grid_map/README.md

@@ -0,0 +1,11 @@
+# probabilistic_occupancy_grid_map
+
+## Purpose
+
+This package outputs the probability of having an obstacle as occupancy grid map.
+![pointcloud_based_occupancy_grid_map_sample_image](./image/pointcloud_based_occupancy_grid_map_sample_image.gif)
+
+## References/External links
+
+- [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md)
+- [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md)

sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp → sensing/probabilistic_occupancy_grid_map/include/cost_value.hpp

@@ -48,8 +48,8 @@
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
 
-#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_
-#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_
+#ifndef COST_VALUE_HPP_
+#define COST_VALUE_HPP_
 
 #include <algorithm>
 
@@ -74,4 +74,4 @@ struct CostTranslationTable
 static const CostTranslationTable cost_translation_table;
 }  // namespace occupancy_cost_value
 
-#endif  // LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_
+#endif  // COST_VALUE_HPP_