fix(image_projection_based_fusion): fix pointpainting error caused by…

… empty pointcloud (#4427)

* fix(image_projection_based_fusion): fix pointpainting error caused by empty pointcloud

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -180,6 +180,12 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
 
 void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg)
 {
+  if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
+    return;
+  }
+
   sensor_msgs::msg::PointCloud2 tmp;
   tmp = painted_pointcloud_msg;
 
@@ -232,6 +238,12 @@ void PointPaintingFusionNode::fuseOnSingleImage(
   const sensor_msgs::msg::CameraInfo & camera_info,
   sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg)
 {
+  if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
+    return;
+  }
+
   auto num_bbox = (input_roi_msg.feature_objects).size();
   if (num_bbox == 0) {
     return;
@@ -342,6 +354,12 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte
     return;
   }
 
+  if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
+    return;
+  }
+
   std::vector<centerpoint::Box3D> det_boxes3d;
   bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d);
   if (!is_success) {