fix(pointcloud_preprocessor): empty pointcloud bug

autowarefoundation · Feb 10, 2022 · b8df615 · b8df615
1 parent eb599ae
commit b8df615
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp
@@ -354,7 +354,10 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-
+  if (input_ptr->data.empty()) {
+    RCLCPP_WARN(get_logger(), "input pointcloud is empty.");
+    return;
+  }
   sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
   convertToXYZICloud(input_ptr, xyzi_input_ptr);
 

diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp
@@ -87,12 +87,14 @@ void DualReturnOutlierFilterComponent::filter(
   PointCloud2 & output)
 {
   boost::mutex::scoped_lock lock(mutex_);
+  if (input->data.empty()) {
+    output.header = input->header;
+    RCLCPP_WARN(get_logger(), "input pointcloud is empty.");
+    return;
+  }
   pcl::PointCloud<return_type_cloud::PointXYZIRADT>::Ptr pcl_input(
     new pcl::PointCloud<return_type_cloud::PointXYZIRADT>);
   pcl::fromROSMsg(*input, *pcl_input);
-  if (pcl_input->points.empty()) {
-    return;
-  }
 
   uint32_t vertical_bins = vertical_bins_;
   uint32_t horizontal_bins = 36;