diff --git a/nodes/usb_cam_node.cpp b/nodes/usb_cam_node.cpp index 55d172b3..d28271f9 100644 --- a/nodes/usb_cam_node.cpp +++ b/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 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; } }