diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index e205984e146d1..c0a373d4ca239 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -76,7 +76,6 @@ struct Det2dStatus double input_offset_ms = 0.0; // cache std::map cached_det2d_msgs; - std::unique_ptr mtx_ptr; }; template @@ -108,7 +107,6 @@ class FusionNode : public rclcpp::Node // 2d detection management methods bool checkAllDet2dFused() { - std::lock_guard lock_det2d_flags(mutex_det2d_flags_); for (const auto & det2d : det2d_list_) { if (!det2d.is_fused) { return false; @@ -116,18 +114,6 @@ class FusionNode : public rclcpp::Node } return true; } - void setDet2dFused(Det2dStatus & det2d) - { - std::lock_guard lock_det2d_flags(mutex_det2d_flags_); - det2d.is_fused = true; - } - void clearAllDet2dFlags() - { - std::lock_guard lock_det2d_flags(mutex_det2d_flags_); - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } - } // Custom process methods virtual void preprocess(Msg3D & output_msg); @@ -144,7 +130,6 @@ class FusionNode : public rclcpp::Node // 2d detection management std::vector> det2d_list_; - std::mutex mutex_det2d_flags_; // camera projection float approx_grid_cell_w_size_; @@ -163,7 +148,7 @@ class FusionNode : public rclcpp::Node // cache for fusion int64_t cached_det3d_msg_timestamp_; typename Msg3D::SharedPtr cached_det3d_msg_ptr_; - std::mutex mutex_det3d_msg_; + std::mutex mutex_cached_msgs_; // parameters for out_of_scope filter float filter_scope_min_x_; diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 296d72e79adb7..bbe80389321e0 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -148,7 +148,6 @@ FusionNode::FusionNode( // 2d detection status initialization det2d_list_.resize(rois_number_); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - det2d_list_.at(roi_i).mtx_ptr = std::make_unique(); det2d_list_.at(roi_i).id = roi_i; det2d_list_.at(roi_i).project_to_unrectified_image = point_project_to_unrectified_image.at(roi_i); @@ -261,13 +260,16 @@ void FusionNode::subCallback( // PROCESS: if the main message is remained (and roi is not collected all) publish the main // message may processed partially with arrived 2d rois stop_watch_ptr_->toc("processing_time", true); - std::lock_guard lock_det3d(mutex_det3d_msg_); exportProcess(); // reset flags - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } + std::lock_guard lock(mutex_cached_msgs_); + // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); @@ -291,7 +293,6 @@ void FusionNode::subCallback( // for loop for each roi for (auto & det2d : det2d_list_) { const auto roi_i = det2d.id; - std::lock_guard lock_det2d(*(det2d.mtx_ptr)); // check camera info if (det2d.camera_projector_ptr == nullptr) { @@ -333,7 +334,7 @@ void FusionNode::subCallback( fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); det2d_msgs.erase(matched_stamp); - setDet2dFused(det2d); + det2d.is_fused = true; // add timestamp interval for debug if (debug_publisher_) { @@ -348,7 +349,6 @@ void FusionNode::subCallback( } // PROCESS: check if the fused message is ready to publish - std::lock_guard lock_det3d(mutex_det3d_msg_); cached_det3d_msg_timestamp_ = timestamp_nsec; cached_det3d_msg_ptr_ = output_msg; if (checkAllDet2dFused()) { @@ -356,7 +356,9 @@ void FusionNode::subCallback( exportProcess(); // reset flags - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } else { // if all of rois are not collected, publish the old Msg(if exists) and cache the // current Msg @@ -374,14 +376,11 @@ void FusionNode::roiCallback( stop_watch_ptr_->toc("processing_time", true); auto & det2d = det2d_list_.at(roi_i); - std::lock_guard lock_det2d(*(det2d.mtx_ptr)); int64_t timestamp_nsec = (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; // if cached Msg exist, try to match if (cached_det3d_msg_ptr_ != nullptr) { - std::lock_guard lock_det3d(mutex_det3d_msg_); - int64_t new_stamp = cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); @@ -401,7 +400,7 @@ void FusionNode::roiCallback( } // PROCESS: fuse the main message with the roi message fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); - setDet2dFused(det2d); + det2d.is_fused = true; if (debug_publisher_) { double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; @@ -416,7 +415,9 @@ void FusionNode::roiCallback( if (checkAllDet2dFused()) { exportProcess(); // reset flags - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; @@ -440,7 +441,7 @@ void FusionNode::timer_callback() using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_det3d_msg_.try_lock()) { + if (mutex_cached_msgs_.try_lock()) { // PROCESS: if timeout, postprocess cached msg if (cached_det3d_msg_ptr_ != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -448,9 +449,11 @@ void FusionNode::timer_callback() } // reset flags whether the message is fused or not - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } - mutex_det3d_msg_.unlock(); + mutex_cached_msgs_.unlock(); } else { // TIMING: retry the process after 10ms try {