Skip to content

Commit

Permalink
fix(image_projection_based_fusion): remove mutex (#9862)
Browse files Browse the repository at this point in the history
refactor: Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
technolojin authored Jan 9, 2025
1 parent b5c008a commit fd23b61
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 24 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,6 @@

#include <map>
#include <memory>
#include <mutex>
#include <set>
#include <string>
#include <utility>
Expand Down Expand Up @@ -127,7 +126,6 @@ class FusionNode : public rclcpp::Node
// cache for fusion
int64_t cached_det3d_msg_timestamp_;
typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
std::mutex mutex_cached_msgs_;

protected:
void setDet2DStatus(std::size_t rois_number);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -280,8 +280,6 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
}
}

std::lock_guard<std::mutex> lock(mutex_cached_msgs_);

// TIMING: reset timer to the timeout time
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double, std::milli>(timeout_ms_));
Expand Down Expand Up @@ -447,28 +445,16 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::timer_callback()

using std::chrono_literals::operator""ms;
timer_->cancel();
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);
exportProcess();
}

// reset flags whether the message is fused or not
for (auto & det2d : det2d_list_) {
det2d.is_fused = false;
}
// PROCESS: if timeout, postprocess cached msg
if (cached_det3d_msg_ptr_ != nullptr) {
stop_watch_ptr_->toc("processing_time", true);
exportProcess();
}

mutex_cached_msgs_.unlock();
} else {
// TIMING: retry the process after 10ms
try {
std::chrono::nanoseconds period = 10ms;
setPeriod(period.count());
} catch (rclcpp::exceptions::RCLError & ex) {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what());
}
timer_->reset();
// reset flags whether the message is fused or not
for (auto & det2d : det2d_list_) {
det2d.is_fused = false;
}
}

Expand Down

0 comments on commit fd23b61

Please sign in to comment.