feat(compare_map_filter): add voxel height offset in comparemap

perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml

@@ -3,6 +3,7 @@
     distance_threshold: 0.5
     use_dynamic_map_loading: true
     downsize_ratio_z_axis: 0.5
+    voxel_height_offset: 0.0
     timer_interval_ms: 100
     map_update_distance_threshold: 10.0
     map_loader_radius: 150.0

perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml

@@ -3,6 +3,7 @@
     distance_threshold: 0.5
     use_dynamic_map_loading: true
     downsize_ratio_z_axis: 0.5
+    voxel_height_offset: 0.0
     timer_interval_ms: 100
     map_update_distance_threshold: 10.0
     map_loader_radius: 150.0

perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml

@@ -3,6 +3,7 @@
     distance_threshold: 0.5
     use_dynamic_map_loading: true
     downsize_ratio_z_axis: 0.5
+    voxel_height_offset: 0.0
     timer_interval_ms: 100
     map_update_distance_threshold: 10.0
     map_loader_radius: 150.0

perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp

@@ -42,7 +42,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 public:
   DistanceBasedStaticMapLoader(
     rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, 1.0, 0.0, tf_map_input_frame, mutex)
   {
     RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n");
   }
@@ -57,7 +57,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   DistanceBasedDynamicMapLoader(
     rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group)
-  : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group)
+  : VoxelGridDynamicMapLoader(
+      node, leaf_size, 1.0, 0.0, tf_map_input_frame, mutex, main_callback_group)
   {
     RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n");
   }

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp

@@ -34,7 +34,7 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader
   explicit VoxelBasedApproximateStaticMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
     std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, 0.0, tf_map_input_frame, mutex)
   {
     RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n");
   }
@@ -49,7 +49,7 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader
     std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group)
   : VoxelGridDynamicMapLoader(
-      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
+      node, leaf_size, downsize_ratio_z_axis, 0.0, tf_map_input_frame, mutex, main_callback_group)
   {
     RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n");
   }

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp

@@ -42,9 +42,10 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 
 public:
   explicit VoxelDistanceBasedStaticMapLoader(
-    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
     std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+  : VoxelGridStaticMapLoader(
+      node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex)
   {
     RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n");
   }
@@ -60,11 +61,12 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   /* data */
 public:
   explicit VoxelDistanceBasedDynamicMapLoader(
-    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double distance_threshold,
     std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group)
   : VoxelGridDynamicMapLoader(
-      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
+      node, leaf_size, downsize_ratio_z_axis, distance_threshold, tf_map_input_frame, mutex,
+      main_callback_group)
   {
     RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n");
   }

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp

@@ -97,6 +97,7 @@ class VoxelGridMapLoader
   double voxel_leaf_size_;
   double voxel_leaf_size_z_;
   double downsize_ratio_z_axis_;
+  double voxel_height_offset_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr downsampled_map_pub_;
   bool debug_ = false;
 
@@ -105,7 +106,7 @@ class VoxelGridMapLoader
   typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
   typedef typename PointCloud::Ptr PointCloudPtr;
   explicit VoxelGridMapLoader(
-    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
     std::string * tf_map_input_frame, std::mutex * mutex);
 
   virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0;
@@ -135,7 +136,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader
 
 public:
   explicit VoxelGridStaticMapLoader(
-    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
     std::string * tf_map_input_frame, std::mutex * mutex);
   virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map);
   virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold);
@@ -191,7 +192,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
 
 public:
   explicit VoxelGridDynamicMapLoader(
-    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
     std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group);
   void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose);

perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp

@@ -46,16 +46,18 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
     RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
     return;
   }
+  double voxel_height_offset = declare_parameter<double>("voxel_height_offset");
   set_map_in_voxel_grid_ = false;
   if (use_dynamic_map_loading) {
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     voxel_grid_map_loader_ = std::make_unique<VoxelGridDynamicMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
-      main_callback_group);
+      this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
+      &mutex_, main_callback_group);
   } else {
     voxel_grid_map_loader_ = std::make_unique<VoxelGridStaticMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+      this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
+      &mutex_);
   }
   tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_);
   RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str());

perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp

@@ -113,6 +113,7 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC
   distance_threshold_ = declare_parameter<double>("distance_threshold");
   bool use_dynamic_map_loading = declare_parameter<bool>("use_dynamic_map_loading");
   double downsize_ratio_z_axis = declare_parameter<double>("downsize_ratio_z_axis");
+  double voxel_height_offset = declare_parameter<double>("voxel_height_offset");
   if (downsize_ratio_z_axis <= 0.0) {
     RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
     return;
@@ -121,11 +122,12 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     voxel_distance_based_map_loader_ = std::make_unique<VoxelDistanceBasedDynamicMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
-      main_callback_group);
+      this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
+      &mutex_, main_callback_group);
   } else {
     voxel_distance_based_map_loader_ = std::make_unique<VoxelDistanceBasedStaticMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+      this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
+      &mutex_);
   }
 }
 

perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp

@@ -15,11 +15,12 @@
 #include "compare_map_segmentation/voxel_grid_map_loader.hpp"
 
 VoxelGridMapLoader::VoxelGridMapLoader(
-  rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+  rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
   std::string * tf_map_input_frame, std::mutex * mutex)
 : logger_(node->get_logger()),
   voxel_leaf_size_(leaf_size),
-  downsize_ratio_z_axis_(downsize_ratio_z_axis)
+  downsize_ratio_z_axis_(downsize_ratio_z_axis),
+  voxel_height_offset_(voxel_height_offset)
 {
   tf_map_input_frame_ = tf_map_input_frame;
   mutex_ptr_ = mutex;
@@ -240,7 +241,7 @@ bool VoxelGridMapLoader::is_in_voxel(
   if (voxel_index != -1) {  // not empty voxel
     const double dist_x = map->points.at(voxel_index).x - target_point.x;
     const double dist_y = map->points.at(voxel_index).y - target_point.y;
-    const double dist_z = map->points.at(voxel_index).z - target_point.z;
+    const double dist_z = map->points.at(voxel_index).z - target_point.z - voxel_height_offset_;
     // check if the point is inside the distance threshold voxel
     if (
       std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold &&
@@ -252,9 +253,10 @@ bool VoxelGridMapLoader::is_in_voxel(
 }
 
 VoxelGridStaticMapLoader::VoxelGridStaticMapLoader(
-  rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+  rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
   std::string * tf_map_input_frame, std::mutex * mutex)
-: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+: VoxelGridMapLoader(
+    node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex)
 {
   voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_;
   sub_map_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -292,10 +294,11 @@ bool VoxelGridStaticMapLoader::is_close_to_map(
 }
 
 VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
-  rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+  rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
   std::string * tf_map_input_frame, std::mutex * mutex,
   rclcpp::CallbackGroup::SharedPtr main_callback_group)
-: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+: VoxelGridMapLoader(
+    node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex)
 {
   voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_;
   auto timer_interval_ms = node->declare_parameter<int>("timer_interval_ms");