diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index f76af5060316c..9ce34d8482fbd 100644 --- 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::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::subCallback(const typename Msg::ConstSharedPtr input_ // Msg if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(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::roiCallback( if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(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::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 index 3641b42db461a..01ec679a82efc 100644 --- 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 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 index 36f09be943144..366820a725018 100644 --- 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_,