Skip to content

Commit fd23b61

Browse files
authored
fix(image_projection_based_fusion): remove mutex (#9862)
refactor: Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent b5c008a commit fd23b61

File tree

2 files changed

+8
-24
lines changed

2 files changed

+8
-24
lines changed

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@
4040

4141
#include <map>
4242
#include <memory>
43-
#include <mutex>
4443
#include <set>
4544
#include <string>
4645
#include <utility>
@@ -127,7 +126,6 @@ class FusionNode : public rclcpp::Node
127126
// cache for fusion
128127
int64_t cached_det3d_msg_timestamp_;
129128
typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
130-
std::mutex mutex_cached_msgs_;
131129

132130
protected:
133131
void setDet2DStatus(std::size_t rois_number);

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

+8-22
Original file line numberDiff line numberDiff line change
@@ -280,8 +280,6 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
280280
}
281281
}
282282

283-
std::lock_guard<std::mutex> lock(mutex_cached_msgs_);
284-
285283
// TIMING: reset timer to the timeout time
286284
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
287285
std::chrono::duration<double, std::milli>(timeout_ms_));
@@ -447,28 +445,16 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::timer_callback()
447445

448446
using std::chrono_literals::operator""ms;
449447
timer_->cancel();
450-
if (mutex_cached_msgs_.try_lock()) {
451-
// PROCESS: if timeout, postprocess cached msg
452-
if (cached_det3d_msg_ptr_ != nullptr) {
453-
stop_watch_ptr_->toc("processing_time", true);
454-
exportProcess();
455-
}
456448

457-
// reset flags whether the message is fused or not
458-
for (auto & det2d : det2d_list_) {
459-
det2d.is_fused = false;
460-
}
449+
// PROCESS: if timeout, postprocess cached msg
450+
if (cached_det3d_msg_ptr_ != nullptr) {
451+
stop_watch_ptr_->toc("processing_time", true);
452+
exportProcess();
453+
}
461454

462-
mutex_cached_msgs_.unlock();
463-
} else {
464-
// TIMING: retry the process after 10ms
465-
try {
466-
std::chrono::nanoseconds period = 10ms;
467-
setPeriod(period.count());
468-
} catch (rclcpp::exceptions::RCLError & ex) {
469-
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what());
470-
}
471-
timer_->reset();
455+
// reset flags whether the message is fused or not
456+
for (auto & det2d : det2d_list_) {
457+
det2d.is_fused = false;
472458
}
473459
}
474460

0 commit comments

Comments
 (0)