feat(probabilistic_occupancy_grid_map): add synchronized ogm fusion node

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()

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

perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml

@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+    # 1. fusion parameters
+    input_ogm_topics: ["topic1", "topic2"]
+    input_ogm_reliabilities: [0.8, 0.2]
+    fusion_method: "log-odds" # 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

perception/probabilistic_occupancy_grid_map/include/cost_value.hpp

@@ -59,19 +59,48 @@ static const unsigned char NO_INFORMATION = 128;  // 0.5 * 255
 static const unsigned char LETHAL_OBSTACLE = 255;
 static const unsigned char FREE_SPACE = 0;
 
+static const unsigned char OCCUPIED_THRESHOLD = 180;
+static const unsigned char FREE_THRESHOLD = 50;
+
 struct CostTranslationTable
 {
   CostTranslationTable()
   {
     for (int i = 0; i < 256; i++) {
-      const auto value = static_cast<char>(static_cast<float>(i) * 100.f / 255.f);
-      data[i] = std::max(std::min(value, static_cast<char>(99)), static_cast<char>(1));
+      const auto value =
+        static_cast<char>(static_cast<float>(i) * 100.f / 255.f);  // 0-255 to 0-100
+      data[i] =
+        std::max(std::min(value, static_cast<char>(99)), static_cast<char>(1));  // 0-100 to 1-99
     }
   }
   char operator[](unsigned char n) const { return data[n]; }
   char data[256];
 };
+struct InverseCostTranslationTable
+{
+  InverseCostTranslationTable()
+  {
+    // 0-100 to 0-255
+    for (int i = 0; i < 100; i++) {
+      data[i] = static_cast<unsigned char>(i * 255 / 99);
+    }
+  }
+  unsigned char operator[](char n) const
+  {
+    if (n > 99) {
+      return data[99];
+    } else if (n < 1) {
+      return data[1];
+    } else {
+      const unsigned char u_n = static_cast<unsigned char>(n);
+      return data[u_n];
+    }
+  }
+  unsigned char data[100];
+};
+
 static const CostTranslationTable cost_translation_table;
+static const InverseCostTranslationTable inverse_cost_translation_table;
 }  // namespace occupancy_cost_value
 
 #endif  // COST_VALUE_HPP_

perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp

@@ -0,0 +1,66 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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.
+
+#ifndef FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_
+#define FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_
+
+#include "cost_value.hpp"
+
+#include <cmath>
+#include <iostream>
+#include <stdexcept>
+#include <vector>
+
+/*min and max prob threshold to prevent log-odds to diverge*/
+#define EPSILON_PROB 0.03
+
+namespace fusion_policy
+{
+enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER };
+
+unsigned char convertProbabilityToChar(const double & probability);
+double convertCharToProbability(const unsigned char & occupancy);
+std::vector<unsigned char> convertProbabilityToChar(const std::vector<double> & probabilities);
+std::vector<double> convertCharToProbability(const std::vector<unsigned char> & occupancies);
+
+namespace overwrite_fusion
+{
+enum State : unsigned char { UNKNOWN = 0U, FREE = 1U, OCCUPIED = 2U };
+State getApproximateState(const unsigned char & occupancy);
+unsigned char overwriteFusion(const std::vector<unsigned char> & occupancies);
+}  // namespace overwrite_fusion
+
+namespace log_odds_fusion
+{
+double logOddsFusion(const std::vector<double> & probabilities);
+double logOddsFusion(
+  const std::vector<double> & probabilities, const std::vector<double> & weights);
+}  // namespace log_odds_fusion
+
+namespace dempster_shafer_fusion
+{
+struct dempsterShaferOccupancy;
+double dempsterShaferFusion(const std::vector<double> & probability);
+double dempsterShaferFusion(
+  const std::vector<double> & probability, const std::vector<double> & reliability);
+}  // namespace dempster_shafer_fusion
+
+unsigned char singleFrameOccupancyFusion(
+  const std::vector<unsigned char> & occupancy, FusionMethod method);
+unsigned char singleFrameOccupancyFusion(
+  const std::vector<unsigned char> & occupancy, FusionMethod method,
+  const std::vector<double> & reliability);
+}  // namespace fusion_policy
+
+#endif  // FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_

perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp

@@ -0,0 +1,127 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// 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.
+
+#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_
+#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_
+
+#include "fusion/single_frame_fusion_policy.hpp"
+#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp"
+#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp"
+#include "updater/occupancy_grid_map_updater_interface.hpp"
+
+#include <builtin_interfaces/msg/time.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
+#include <message_filters/pass_through.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace synchronized_grid_map_fusion
+{
+
+using costmap_2d::OccupancyGridMapFixedBlindSpot;
+using costmap_2d::OccupancyGridMapLOBFUpdater;
+using costmap_2d::OccupancyGridMapUpdaterInterface;
+using geometry_msgs::msg::Pose;
+
+class GridMapFusionNode : public rclcpp::Node
+{
+public:
+  explicit GridMapFusionNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void onGridMap(
+    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg,
+    const std::string & input_topic);
+  nav_msgs::msg::OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr(
+    const std::string & frame_id, const builtin_interfaces::msg::Time & stamp,
+    const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map);
+
+  nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D(
+    const nav_msgs::msg::OccupancyGrid & occupancy_grid_map);
+  OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap(
+    const nav_msgs::msg::OccupancyGrid & occupancy_grid_map);
+  OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion(
+    std::vector<OccupancyGridMapFixedBlindSpot> & occupancy_grid_maps,
+    const builtin_interfaces::msg::Time latest_stamp, const std::vector<double> & weights);
+
+  void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg);
+
+  void setPeriod(const int64_t new_period);
+  void timer_callback();
+  void publish();
+
+private:
+  // Publisher and Subscribers
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr fused_map_pub_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr single_frame_pub_;
+  std::vector<rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr> grid_map_subs_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{};
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{};
+
+  // Topics manager
+  std::size_t num_input_topics_{1};
+  std::vector<std::string> input_topics_;
+  std::vector<double> input_offset_sec_;
+  std::vector<double> input_topic_weights_;
+  std::map<std::string, double> input_topic_weights_map_;
+
+  // TF
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  // Timer to manage the timeout of the occupancy grid map
+  rclcpp::TimerBase::SharedPtr timer_;
+  double timeout_sec_{};
+  double match_threshold_sec_{};
+
+  // cache for fusion
+  std::mutex mutex_;
+  std::shared_ptr<OccupancyGridMapUpdaterInterface>
+    occupancy_grid_map_updater_ptr_;  // contains fused grid map
+  std::map<std::string, nav_msgs::msg::OccupancyGrid::ConstSharedPtr>
+    gridmap_dict_;  // temporary cache for grid map message
+  std::map<std::string, nav_msgs::msg::OccupancyGrid::ConstSharedPtr>
+    gridmap_dict_tmp_;                        // second cache for grid map message
+  std::map<std::string, double> offset_map_;  // time offset for each grid map
+
+  // grid map parameters
+  std::string map_frame_;
+  std::string base_link_frame_;
+  std::string gridmap_origin_frame_;
+  float fusion_map_length_x_;
+  float fusion_map_length_y_;
+  float fusion_map_resolution_;
+  fusion_policy::FusionMethod fusion_method_;
+};
+
+}  // namespace synchronized_grid_map_fusion
+
+#endif  // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_

perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp

@@ -0,0 +1,48 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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.
+
+#ifndef UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_
+#define UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_
+
+#include "fusion/single_frame_fusion_policy.hpp"
+#include "updater/occupancy_grid_map_updater_interface.hpp"
+#include "utils/utils.hpp"
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+
+// LOBF means: Log Odds Bayes Filter
+
+namespace costmap_2d
+{
+class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface
+{
+public:
+  enum Index : size_t { OCCUPIED = 0U, FREE = 1U };
+  OccupancyGridMapLOBFUpdater(
+    const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution)
+  : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution)
+  {
+  }
+  bool update(const Costmap2D & single_frame_occupancy_grid_map) override;
+  void initRosParam(rclcpp::Node & node) override;
+
+private:
+  inline unsigned char applyLOBF(const unsigned char & z, const unsigned char & o);
+  Eigen::Matrix2f probability_matrix_;
+};
+
+}  // namespace costmap_2d
+
+#endif  // UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_