diff --git a/src/detector.cpp b/src/detector.cpp index dbb258a..21df487 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -45,6 +45,8 @@ class UVDARDetector : public nodelet::Nodelet{ param_loader.loadParam("threshold", _threshold_, 200); + param_loader.loadParam("initial_delay", _initial_delay_, 5.0); + /* subscribe to cameras //{ */ std::vector _camera_topics; param_loader.loadParam("camera_topics", _camera_topics, _camera_topics); @@ -239,18 +241,14 @@ class UVDARDetector : public nodelet::Nodelet{ return; } - if (!uvdf_was_initialized_){ - if (!uvdf_->initDelayed(image->image)){ - ROS_WARN_STREAM_THROTTLE(1.0,"[UVDARDetector]: Failed to initialize, dropping message..."); - return; - } + if (!initial_delay_started_){ initial_delay_start_ = ros::Time::now(); - uvdf_was_initialized_ = true; + initial_delay_started_ = true; } - double initial_delay = 5.0; //seconds. This delay is necessary to avoid strange segmentation faults with software rendering backend for OpenGL used in the buildfarm testing. - if ((ros::Time::now() - initial_delay_start_).toSec() < initial_delay){ - ROS_WARN_STREAM_THROTTLE(1.0, "[UVDARDetector]: Ignoring message for "<< initial_delay <<"s..."); + /* double initial_delay = 5.0; //seconds. This delay is necessary to avoid strange segmentation faults with software rendering backend for OpenGL used in the buildfarm testing. */ + if ((ros::Time::now() - initial_delay_start_).toSec() < _initial_delay_){ + ROS_WARN_STREAM_THROTTLE(1.0, "[UVDARDetector]: Ignoring message for "<< _initial_delay_ <<"s..."); return; } @@ -264,6 +262,15 @@ class UVDARDetector : public nodelet::Nodelet{ { /* std::scoped_lock lock(*mutex_camera_image_[image_index]); */ std::scoped_lock lock(mutex_camera_image_); + + if (!uvdf_was_initialized_){ + if (!uvdf_->initDelayed(image->image)){ + ROS_WARN_STREAM_THROTTLE(1.0,"[UVDARDetector]: Failed to initialize, dropping message..."); + return; + } + uvdf_was_initialized_ = true; + } + images_current_[image_index] = image->image; sun_points_[image_index].clear(); detected_points_[image_index].clear(); @@ -426,6 +433,8 @@ class UVDARDetector : public nodelet::Nodelet{ int _threshold_; + double _initial_delay_ = 5.0; + bool _use_masks_; std::vector _mask_file_names_; std::vector _masks_; @@ -436,6 +445,7 @@ class UVDARDetector : public nodelet::Nodelet{ std::vector timer_process_; bool uvdf_was_initialized_ = false; + bool initial_delay_started_ = false; ros::Time initial_delay_start_;