fix: add s/p hotfix for beta/v0.10.0 (#688)

* fix(image_projection_based_fusion): revert the swap of postprocess and publish (autowarefoundation#4400) fix: revert the swap of postprocess and publish Signed-off-by: tzhong518 <[email protected]> * fix(image_projection_based_fusion): fix pointpainting error caused by empty pointcloud (autowarefoundation#4427) * fix(image_projection_based_fusion): fix pointpainting error caused by empty pointcloud Signed-off-by: yukke42 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: yukke42 <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(traffic_light_occlusion_predictor): bug fix (autowarefoundation#4426) fix traffic_light_occlusion_predictor bug Signed-off-by: Mingyu Li <[email protected]> --------- Signed-off-by: tzhong518 <[email protected]> Signed-off-by: yukke42 <[email protected]> Signed-off-by: Mingyu Li <[email protected]> Co-authored-by: Tao Zhong <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Mingyu1991 <[email protected]>
tier4 · Jul 28, 2023 · 927c9de · 927c9de
1 parent 6cc8313
commit 927c9de
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 5 deletions.
diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp
@@ -166,8 +166,8 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
   if (sub_std_pair_.second != nullptr) {
     stop_watch_ptr_->toc("processing_time", true);
     timer_->cancel();
-    publish(*(sub_std_pair_.second));
     postprocess(*(sub_std_pair_.second));
+    publish(*(sub_std_pair_.second));
     sub_std_pair_.second = nullptr;
     std::fill(is_fused_.begin(), is_fused_.end(), false);
 
@@ -260,8 +260,8 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
   // Msg
   if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast<int>(rois_number_)) {
     timer_->cancel();
-    publish(*output_msg);
     postprocess(*output_msg);
+    publish(*output_msg);
     std::fill(is_fused_.begin(), is_fused_.end(), false);
     sub_std_pair_.second = nullptr;
 
@@ -322,8 +322,8 @@ void FusionNode<Msg, Obj>::roiCallback(
 
       if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast<int>(rois_number_)) {
         timer_->cancel();
-        publish(*(sub_std_pair_.second));
         postprocess(*(sub_std_pair_.second));
+        publish(*(sub_std_pair_.second));
         std::fill(is_fused_.begin(), is_fused_.end(), false);
         sub_std_pair_.second = nullptr;
 
@@ -362,8 +362,8 @@ void FusionNode<Msg, Obj>::timer_callback()
     if (sub_std_pair_.second != nullptr) {
       stop_watch_ptr_->toc("processing_time", true);
 
-      publish(*(sub_std_pair_.second));
       postprocess(*(sub_std_pair_.second));
+      publish(*(sub_std_pair_.second));
 
       // add processing time for debug
       if (debug_publisher_) {

diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/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) {

diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp
@@ -117,7 +117,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback(
   if (
     in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr ||
     in_roi_msg->rois.size() != in_signal_msg->signals.size()) {
-    occlusion_ratios.resize(in_roi_msg->rois.size(), 0);
+    occlusion_ratios.resize(out_msg.signals.size(), 0);
   } else {
     cloud_occlusion_predictor_->predict(
       in_cam_info_msg, in_roi_msg, in_cloud_msg, tf_buffer_, traffic_light_position_map_,