feat: support single frame mode (#496)

* feat: support single frame mode in occ grid

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>

sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp

@@ -93,6 +93,7 @@ class OccupancyGridMapNode : public rclcpp::Node
   std::string map_frame_;
   std::string base_link_frame_;
   bool use_height_filter_;
+  bool enable_single_frame_mode_;
 };
 
 }  // namespace occupancy_grid_map

sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp

@@ -36,7 +36,7 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface
     probability_matrix_(Index::FREE, Index::FREE) = 0.8;
     probability_matrix_(Index::OCCUPIED, Index::FREE) = 1.0 - probability_matrix_(FREE, FREE);
   }
-  bool update(const Costmap2D & oneshot_occupancy_grid_map) override;
+  bool update(const Costmap2D & single_frame_occupancy_grid_map) override;
 
 private:
   inline unsigned char applyBBF(const unsigned char & z, const unsigned char & o);

sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp

@@ -31,7 +31,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D
   {
   }
   virtual ~OccupancyGridMapUpdaterInterface() = default;
-  virtual bool update(const Costmap2D & oneshot_occupancy_grid_map) = 0;
+  virtual bool update(const Costmap2D & single_frame_occupancy_grid_map) = 0;
 };
 
 }  // namespace costmap_2d

sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp

@@ -119,6 +119,7 @@ OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_opti
   map_frame_ = declare_parameter("map_frame", "map");
   base_link_frame_ = declare_parameter("base_link_frame", "base_link");
   use_height_filter_ = declare_parameter("use_height_filter", true);
+  enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false);
   const double map_length{declare_parameter("map_length", 100.0)};
   const double map_resolution{declare_parameter("map_resolution", 0.5)};
   const bool input_obstacle_pointcloud{declare_parameter("input_obstacle_pointcloud", true)};
@@ -221,24 +222,32 @@ void OccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw(
     return;
   }
 
-  // Create oneshot occupancy grid map
-  OccupancyGridMap oneshot_occupancy_grid_map(
+  // Create single frame occupancy grid map
+  OccupancyGridMap single_frame_occupancy_grid_map(
     occupancy_grid_map_updater_ptr_->getSizeInCellsX(),
     occupancy_grid_map_updater_ptr_->getSizeInCellsY(),
     occupancy_grid_map_updater_ptr_->getResolution());
-  oneshot_occupancy_grid_map.updateOrigin(
-    pose.position.x - oneshot_occupancy_grid_map.getSizeInMetersX() / 2,
-    pose.position.y - oneshot_occupancy_grid_map.getSizeInMetersY() / 2);
-  oneshot_occupancy_grid_map.updateFreespaceCells(trans_raw_pc);
-  oneshot_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose);
-  oneshot_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc);
-
-  // Update with bayes filter
-  occupancy_grid_map_updater_ptr_->update(oneshot_occupancy_grid_map);
-
-  // publish
-  occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr(
-    map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, *occupancy_grid_map_updater_ptr_));
+  single_frame_occupancy_grid_map.updateOrigin(
+    pose.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2,
+    pose.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2);
+  single_frame_occupancy_grid_map.updateFreespaceCells(trans_raw_pc);
+  single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose);
+  single_frame_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc);
+
+  if (enable_single_frame_mode_) {
+    // publish
+    occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr(
+      map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z,
+      single_frame_occupancy_grid_map));
+  } else {
+    // Update with bayes filter
+    occupancy_grid_map_updater_ptr_->update(single_frame_occupancy_grid_map);
+
+    // publish
+    occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr(
+      map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z,
+      *occupancy_grid_map_updater_ptr_));
+  }
 }
 
 OccupancyGrid::UniquePtr OccupancyGridMapNode::OccupancyGridMapToMsgPtr(

sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp

@@ -45,13 +45,14 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF(
     static_cast<unsigned char>(254));
 }
 
-bool OccupancyGridMapBBFUpdater::update(const Costmap2D & oneshot_occupancy_grid_map)
+bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map)
 {
-  updateOrigin(oneshot_occupancy_grid_map.getOriginX(), oneshot_occupancy_grid_map.getOriginY());
+  updateOrigin(
+    single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY());
   for (unsigned int x = 0; x < getSizeInCellsX(); x++) {
     for (unsigned int y = 0; y < getSizeInCellsY(); y++) {
       unsigned int index = getIndex(x, y);
-      costmap_[index] = applyBBF(oneshot_occupancy_grid_map.getCost(x, y), costmap_[index]);
+      costmap_[index] = applyBBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]);
     }
   }
   return true;