diff --git a/include/detect/uv_led_detect_fast_gpu.cpp b/include/detect/uv_led_detect_fast_gpu.cpp index 8555ff6..f2cff79 100644 --- a/include/detect/uv_led_detect_fast_gpu.cpp +++ b/include/detect/uv_led_detect_fast_gpu.cpp @@ -137,7 +137,7 @@ bool uvdar::UVDARLedDetectFASTGPU::processImage(const cv::Mat i_image, std::vect image_size = i_image.size(); /* std::cerr << "[UVDARDetectorFASTGPU]: First image, initializing..." << std::endl; */ init(); - /* return false; */ + return false; } if (mask_id >= (int)(masks_.size())) { diff --git a/src/detector.cpp b/src/detector.cpp index 35874d0..bbc3922 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -71,6 +71,7 @@ class UVDARDetector : public nodelet::Nodelet{ } //} + // Create callbacks, timers and process objects for each camera for (unsigned int i = 0; i < _camera_count_; ++i) { image_callback_t callback = [image_index=i,this] (const sensor_msgs::ImageConstPtr& image_msg) { @@ -87,6 +88,9 @@ class UVDARDetector : public nodelet::Nodelet{ detected_points_.push_back(std::vector()); sun_points_.push_back(std::vector()); + image_yet_received_.push_back(false); + initial_delay_.push_back(ros::Time::now()); + /* mutex_camera_image_.push_back(std::make_unique()); */ ROS_INFO("[UVDARDetector]: Initializing FAST-based marker detection..."); @@ -141,6 +145,7 @@ class UVDARDetector : public nodelet::Nodelet{ } + ROS_INFO("[UVDARDetector]: Waiting for time..."); ros::Time::waitForValid(); @@ -215,6 +220,18 @@ class UVDARDetector : public nodelet::Nodelet{ * @param image_index - index of the camera that produced this image */ void processSingleImage([[maybe_unused]] const ros::TimerEvent& te, const cv_bridge::CvImageConstPtr image, int image_index) { + + if (!image_yet_received_[image_index]){ + initial_delay_[image_index] = ros::Time::now(); + image_yet_received_[image_index] = true; + } + + if ((ros::Time::now() - initial_delay_[image_index]).toSec() < 2.0){ + ROS_WARN_STREAM_THROTTLE(1.0, "[UVDARDetector]: Ignoring message for 2s..."); + return; + } + + if (!initialized_){ ROS_WARN_STREAM_THROTTLE(1.0,"[UVDARDetector]: Not yet initialized, dropping message..."); return; @@ -393,6 +410,10 @@ class UVDARDetector : public nodelet::Nodelet{ std::mutex mutex_pub_; std::vector timer_process_; + std::vector image_yet_received_; + std::vector initial_delay_; + + };