Block manual cycle after init cycle

Block manual camera cycling until the initial cycle
is completed.

nodes/usb_cam_node.cpp

@@ -63,7 +63,7 @@ const int WAIT_CHANGING_AUTO_EXPOSURE_SEC = 2;
 const double AUTO_RESET_EXPOSURE_PERIOD = 60.0;
 
 //! \brief Timer period in seconds between calls to reset camera exposure setting
-const double ONE_SHOT_RESET_EXPOSURE_WAIT = 5.0;
+const double ONE_SHOT_RESET_EXPOSURE_WAIT = 1.0;
 
 class UsbCamNode
 {
@@ -89,6 +89,7 @@ class UsbCamNode
   int image_width_, image_height_, framerate_, bits_per_pixel_, exposure_, brightness_, contrast_, saturation_,
       sharpness_, focus_, white_balance_, gain_, power_line_frequency_, gamma_, backlight_compensation_;
   bool autofocus_, autoexposure_, auto_white_balance_;
+  bool init_cam_reset_;
   boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
 
   UsbCam cam_;
@@ -103,6 +104,17 @@ class UsbCamNode
 
   bool reset_exposure_call(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res )
   {
+    ros::Rate srv_rate(5);
+    bool timeout = 20.0;
+    float start_time = ros::Time::now().toSec();
+    while (node_.ok() && (ros::Time::now().toSec() - start_time < timeout))
+    {
+      if (init_cam_reset_)
+      {
+        break;
+      }
+      srv_rate.sleep();
+    }
     resetExposureSettings();
     return true;
   }
@@ -165,6 +177,8 @@ class UsbCamNode
     node_.param("camera_info_url", camera_info_url_, std::string(""));
     cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
 
+    init_cam_reset_ = false;
+
     // create Services
     service_start_ = node_.advertiseService("start_capture", &UsbCamNode::service_start_cap, this);
     service_stop_ = node_.advertiseService("stop_capture", &UsbCamNode::service_stop_cap, this);
@@ -394,6 +408,7 @@ class UsbCamNode
     if (enable_auto_reset_exposure_)
     {
       resetExposureSettings();
+      init_cam_reset_ = true;
     }
   }