diff --git a/cfg/Zed.cfg b/cfg/Zed.cfg index 64b4f794..465a7b4f 100755 --- a/cfg/Zed.cfg +++ b/cfg/Zed.cfg @@ -6,5 +6,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80, 1, 100) +gen.add("exposure", int_t, 1, "Exposure value when manual controlled", 100, 0, 100); +gen.add("gain", int_t, 2, "Gain value when manual controlled", 50, 0, 100); +gen.add("auto_exposure", bool_t, 3, "Enable/Disable auto control of exposure and gain", True); exit(gen.generate(PACKAGE, "zed_wrapper", "Zed")) diff --git a/launch/zed_camera.launch b/launch/zed_camera.launch index fc26c1d6..386257d0 100644 --- a/launch/zed_camera.launch +++ b/launch/zed_camera.launch @@ -60,6 +60,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + diff --git a/src/zed_wrapper_nodelet.cpp b/src/zed_wrapper_nodelet.cpp index fb20fb5e..170fcdd5 100644 --- a/src/zed_wrapper_nodelet.cpp +++ b/src/zed_wrapper_nodelet.cpp @@ -147,6 +147,10 @@ namespace zed_wrapper { // flags int confidence; + int exposure; + int gain; + bool autoExposure; + bool triggerAutoExposure; bool computeDepth; bool grabbing = false; int openniDepthMode = 0; // 16 bit UC data in mm else 32F in m, for more info http://www.ros.org/reps/rep-0118.html @@ -483,8 +487,26 @@ namespace zed_wrapper { } void callback(zed_wrapper::ZedConfig &config, uint32_t level) { - NODELET_INFO("Reconfigure confidence : %d", config.confidence); - confidence = config.confidence; + switch (level) { + case 0: + NODELET_INFO("Reconfigure confidence : %d", config.confidence); + confidence = config.confidence; + break; + case 1: + NODELET_INFO("Reconfigure exposure : %d", config.exposure); + exposure = config.exposure; + break; + case 2: + NODELET_INFO("Reconfigure gain : %d", config.gain); + gain = config.gain; + break; + case 3: + NODELET_INFO("Reconfigure auto control of exposure and gain : %s", config.auto_exposure ? "Enable" : "Disable"); + autoExposure = config.auto_exposure; + if (autoExposure) + triggerAutoExposure = true; + break; + } } void device_poll() { @@ -608,6 +630,23 @@ namespace zed_wrapper { old_t = ros::Time::now(); + if (autoExposure) { + // getCameraSettings() can't check status of auto exposure + // triggerAutoExposure is used to execute setCameraSettings() only once + if (triggerAutoExposure) { + zed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, 0, true); + triggerAutoExposure = false; + } + } else { + int actual_exposure = zed.getCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE); + if (actual_exposure != exposure) + zed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, exposure); + + int actual_gain = zed.getCameraSettings(sl::CAMERA_SETTINGS_GAIN); + if (actual_gain != gain) + zed.setCameraSettings(sl::CAMERA_SETTINGS_GAIN, gain); + } + // Publish the left == rgb image if someone has subscribed to if (left_SubNumber > 0 || rgb_SubNumber > 0) { // Retrieve RGBA Left image @@ -891,14 +930,18 @@ namespace zed_wrapper { serial_number = zed.getCameraInformation().serial_number; - //Reconfigure confidence + //Reconfigure parameters server = boost::make_shared> (); dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&ZEDWrapperNodelet::callback, this, _1, _2); server->setCallback(f); nh_ns.getParam("confidence", confidence); - + nh_ns.getParam("exposure", exposure); + nh_ns.getParam("gain", gain); + nh_ns.getParam("auto_exposure", autoExposure); + if (autoExposure) + triggerAutoExposure = true; // Create all the publishers // Image publishers