From 38d16bad86424ebd1543db5813920ca987c16fa6 Mon Sep 17 00:00:00 2001
From: Bey9434 <aluca9434@gmail.com>
Date: Mon, 26 Aug 2024 23:37:41 +0900
Subject: [PATCH] add area & self cropping

---
 .cspell.json                                  |  1 +
 .../config/pointcloud_preprocessor.param.yaml |  2 +-
 .../cropbox_filter.hpp                        | 34 +++++++++++--------
 .../src/pointcloud_preprocessor.cpp           |  5 +--
 4 files changed, 25 insertions(+), 17 deletions(-)

diff --git a/.cspell.json b/.cspell.json
index c5f30ee..51c60a7 100644
--- a/.cspell.json
+++ b/.cspell.json
@@ -47,6 +47,7 @@
     "coef",
     "colcon",
     "costmap",
+    "cropbox",
     "DCMAKE",
     "downsample",
     "downsampled",
diff --git a/src/sensing/pointcloud_preprocessor/config/pointcloud_preprocessor.param.yaml b/src/sensing/pointcloud_preprocessor/config/pointcloud_preprocessor.param.yaml
index 89dd703..bb7c318 100644
--- a/src/sensing/pointcloud_preprocessor/config/pointcloud_preprocessor.param.yaml
+++ b/src/sensing/pointcloud_preprocessor/config/pointcloud_preprocessor.param.yaml
@@ -2,4 +2,4 @@
   pointcloud_preprocessor:
     ros__parameters:
       target_frame_id: base_link
-      leaf_size: 0.05
+      leaf_size: 0.1
diff --git a/src/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/cropbox_filter.hpp b/src/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/cropbox_filter.hpp
index acc4358..06ee1b2 100644
--- a/src/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/cropbox_filter.hpp
+++ b/src/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/cropbox_filter.hpp
@@ -49,34 +49,40 @@ class CropBoxFilter
 
     // 必要であれば、debug_parameters()を呼び出して詳細をログ出力
     debug_parameters();
-
-    // debug_parameters();
   }
 
   pcl::PointCloud<pcl::PointXYZ>::Ptr crop_box(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_output)
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_output,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr & cropped_cloud)
   {
-    pcl::CropBox<pcl::PointXYZ> crop_box_filter;
-
-    Eigen::Vector4f min_pt(
+    Eigen::Vector4f self_min_pt(
       -robot_info_.raw_info.rear_overhang_m,   // x軸負方向（後方）
       -robot_info_.raw_info.right_overhang_m,  // y軸負方向（左側）
       -robot_info_.raw_info.robot_height_m,    // z軸負方向（下方）
       1.0);
 
-    Eigen::Vector4f max_pt(
+    Eigen::Vector4f self_max_pt(
       robot_info_.raw_info.front_overhang_m,  // x軸正方向（前方）
       robot_info_.raw_info.left_overhang_m,   // y軸正方向（右側）
       robot_info_.raw_info.robot_height_m,    // z軸正方向（上方）
       1.0);
 
-    crop_box_filter.setInputCloud(pcl_output);
-    crop_box_filter.setMin(min_pt);
-    crop_box_filter.setMax(max_pt);
-
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>());
-    crop_box_filter.setNegative(true);
-    crop_box_filter.filter(*cropped_cloud);
+    pcl::CropBox<pcl::PointXYZ> self_crop_box_filter;
+
+    self_crop_box_filter.setInputCloud(pcl_output);
+    self_crop_box_filter.setMin(self_min_pt);
+    self_crop_box_filter.setMax(self_max_pt);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr self_cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    self_crop_box_filter.setNegative(true);
+    self_crop_box_filter.filter(*self_cropped_cloud);
+
+    pcl::CropBox<pcl::PointXYZ> area_crop_box_filter;
+    area_crop_box_filter.setInputCloud(self_cropped_cloud);
+    Eigen::Vector4f area_min_pt(-5.0, -5.0, -5.0, 1.0);
+    Eigen::Vector4f area_max_pt(5.0, 5.0, 5.0, 1.0);
+    area_crop_box_filter.setMin(area_min_pt);
+    area_crop_box_filter.setMax(area_max_pt);
+    area_crop_box_filter.filter(*cropped_cloud);
 
     return cropped_cloud;
   }
diff --git a/src/sensing/pointcloud_preprocessor/src/pointcloud_preprocessor.cpp b/src/sensing/pointcloud_preprocessor/src/pointcloud_preprocessor.cpp
index 902e4af..7a2989b 100644
--- a/src/sensing/pointcloud_preprocessor/src/pointcloud_preprocessor.cpp
+++ b/src/sensing/pointcloud_preprocessor/src/pointcloud_preprocessor.cpp
@@ -65,11 +65,12 @@ void PointCloudPreprocessor::process_pointcloud(const sensor_msgs::msg::PointClo
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>());
   pcl::transformPointCloud(*pcl_input, *pcl_output, transform_matrix);
 
-  // セルフクロッピング処理
+  // クロッピング処理
   cropbox_filter::CropBoxFilter crop_box_filter(*this);
   // デバッグ用のパラメータ確認メソッドを呼び出す。デバッグ用。
   // crop_box_filter.debug_parameters();
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud = crop_box_filter.crop_box(pcl_output);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+  crop_box_filter.crop_box(pcl_output, cropped_cloud);
 
   // ダウンサンプリング
   pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;