From 1ba224f375bab16b9d17bbf54a3d3719bd17042d Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 12 Jan 2024 19:39:06 +0900 Subject: [PATCH] fix: publish ground pc Signed-off-by: badai-nguyen --- .../scan_ground_filter_nodelet.hpp | 5 +++- .../src/scan_ground_filter_nodelet.cpp | 29 +++++++++++++++++-- 2 files changed, 30 insertions(+), 4 deletions(-) diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 980565fe2144b..90c82cf6c2c95 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/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::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::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr); + pcl::PointCloud::Ptr out_object_cloud_ptr, pcl::PointCloud::Ptr out_ground_cloud_ptr); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..9804f489a3e2a 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/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("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::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr) + pcl::PointCloud::Ptr out_object_cloud_ptr, pcl::PointCloud::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::Ptr no_ground_cloud_ptr(new pcl::PointCloud); no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size()); + pcl::PointCloud::Ptr ground_cloud_ptr(new pcl::PointCloud); + 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(); + 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(); pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr);