video_stream: implement auto focus

ros-drivers · Nov 12, 2020 · 69398a3 · 69398a3
1 parent d3d9045
commit 69398a3
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/cfg/VideoStream.cfg b/cfg/VideoStream.cfg
@@ -29,6 +29,8 @@ gen.add("hue", double_t, LEVEL.RUNNING, "Target hue", 0.5, 0.0, 1.0)
 gen.add("saturation", double_t, LEVEL.RUNNING, "Target saturation", 0.64, 0.0, 1.0)
 gen.add("auto_exposure", bool_t, LEVEL.RUNNING, "Target auto exposure", True)
 gen.add("exposure", double_t, LEVEL.RUNNING, "Target exposure", 0.5, 0.0, 1.0)
+gen.add("auto_focus", bool_t, LEVEL.RUNNING, "Target auto focus", True)
+gen.add("focus", double_t, LEVEL.RUNNING, "Target focus", 0.5, 0.0, 1.0)
 gen.add("loop_videofile", bool_t, LEVEL.RUNNING, "Loop videofile", False)
 gen.add("reopen_on_read_failure", bool_t, LEVEL.RUNNING, "Re-open camera device on read failure", False)
 gen.add("output_encoding", str_t, LEVEL.NORMAL, "Output encoding", "bgr8")

diff --git a/src/video_stream.cpp b/src/video_stream.cpp
@@ -287,6 +287,14 @@ virtual void subscribe() {
   cap->set(cv::CAP_PROP_HUE, latest_config.hue);
   cap->set(cv::CAP_PROP_SATURATION, latest_config.saturation);
 
+  if (latest_config.auto_focus) {
+      cap->set(cv::CAP_PROP_AUTOFOCUS, 1);
+      latest_config.exposure = 0.5;
+  } else {
+      cap->set(cv::CAP_PROP_AUTOFOCUS, 0);
+      cap->set(cv::CAP_PROP_FOCUS, latest_config.focus);
+  }
+
   if (latest_config.auto_exposure) {
     cap->set(cv::CAP_PROP_AUTO_EXPOSURE, 0.75);
     latest_config.exposure = 0.5;