From 3fafc9575139a07ee6ca8f25ed29ef9e5d2a5a9e Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 28 Jun 2023 12:45:37 +0900 Subject: [PATCH] fix(concatedate_data): add error handling when sensor points is empty (backport #3814) (#526) fix(concatedate_date): add error handling when sensor points is empty (#3814) fix(concatedate_date): add error handling when sensor points are empty --- .../src/concatenate_data/concatenate_data_nodelet.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index e2d3b44e32299..8bae5f943548a 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -307,6 +307,13 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; + + if (input_ptr->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width);