fix: publish ground pc

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -176,6 +176,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   size_t radial_dividers_num_;
   VehicleInfo vehicle_info_;
 
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ground_pc_pub_;
+
+
   /*!
    * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame
    * @param[in] in_target_frame Coordinate system to perform transform
@@ -241,7 +244,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    */
   void extractObjectPoints(
     const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
-    pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_ground_cloud_ptr);
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -63,7 +63,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
     virtual_lidar_z_ = vehicle_info_.vehicle_height_m;
     grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_);
   }
-
+  ground_pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("output/ground_pointcloud", 1);
   using std::placeholders::_1;
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&ScanGroundFilterComponent::onParameter, this, _1));
@@ -530,11 +530,27 @@ void ScanGroundFilterComponent::classifyPointCloud(
 
 void ScanGroundFilterComponent::extractObjectPoints(
   const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
-  pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr)
+  pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_ground_cloud_ptr)
 {
+  pcl::PointIndices::Ptr ground_indices(new pcl::PointIndices);
+  ground_indices->indices = in_indices.indices;
+  std::sort(ground_indices->indices.begin(), ground_indices->indices.end());
+
   for (const auto & i : in_indices.indices) {
     out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
   }
+  int prev_index = 0;
+  for (auto curr_idx = ground_indices->indices.begin()+1; curr_idx != ground_indices->indices.end(); ++curr_idx) {
+    if (*curr_idx - prev_index > 1) {
+      for (auto i = prev_index+1; i < *curr_idx; ++i) {
+        out_ground_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
+      }
+    }
+    prev_index = *curr_idx;
+  }
+  for (size_t i = prev_index+1; i < in_cloud_ptr->points.size(); ++i) {
+    out_ground_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
+  }
 }
 
 void ScanGroundFilterComponent::filter(
@@ -551,6 +567,9 @@ void ScanGroundFilterComponent::filter(
   pcl::PointIndices no_ground_indices;
   pcl::PointCloud<pcl::PointXYZ>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size());
+  pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size());
+
 
   if (elevation_grid_mode_) {
     convertPointcloudGridScan(current_sensor_cloud_ptr, radial_ordered_points);
@@ -560,7 +579,11 @@ void ScanGroundFilterComponent::filter(
     classifyPointCloud(radial_ordered_points, no_ground_indices);
   }
 
-  extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr);
+  extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr, ground_cloud_ptr);
+  auto ground_cloud_msg_ptr = std::make_shared<PointCloud2>();
+  pcl::toROSMsg(*ground_cloud_ptr, *ground_cloud_msg_ptr);
+  ground_cloud_msg_ptr->header = input->header;
+  ground_pc_pub_->publish(*ground_cloud_msg_ptr);
 
   auto no_ground_cloud_msg_ptr = std::make_shared<PointCloud2>();
   pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr);