Adding a static delay from first received camera image till we start …

…processing them. This may alleviate issues caused when the software shader starts running immediately after loading it in.

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())) {

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<cv::Point>());
       sun_points_.push_back(std::vector<cv::Point>());
 
+      image_yet_received_.push_back(false);
+      initial_delay_.push_back(ros::Time::now());
+
       /* mutex_camera_image_.push_back(std::make_unique<std::mutex>()); */
 
       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<ros::Timer> timer_process_;
 
+  std::vector<bool> image_yet_received_;
+  std::vector<ros::Time> initial_delay_;
+
+
 };