Add support for autofocus

photon-client/src/views/CamerasView.vue

@@ -299,6 +299,19 @@
                   Download Target
                 </v-btn>
               </v-col>
+              <v-col>
+                <v-btn
+                  color="secondary"
+                  small
+                  style="width: 100%;"
+                  @click="focusCamera"
+                >
+                <v-icon left>
+                  mdi-camera-iris
+                </v-icon>
+                Focus Camera
+                </v-btn>
+              </v-col>
               <v-col>
                 <v-btn
                   color="secondary"
@@ -330,7 +343,7 @@
             style="border-radius: 5px;"
           />
           <v-dialog
-            v-model="snack"
+            v-model="calibrationSnack"
             width="500px"
             :persistent="true"
           >
@@ -387,6 +400,77 @@
           </v-dialog>
         </template>
       </v-col>
+      <v-col
+        class="pl-md-3 pt-3 pt-md-0"
+        cols="12"
+        md="5"
+      >
+        <template>
+          <CVimage
+            :id="cameras-focus"
+            :idx=1
+            :disconnected="!$store.state.backendConnected"
+            scale="100"
+            style="border-radius: 5px;"
+          />
+          <v-dialog
+            v-model="focusSnack"
+            width="500px"
+            :persistent="true"
+          >
+            <v-card
+              color="primary"
+              dark
+            >
+              <v-card-title> Camera Autofocus </v-card-title>
+              <div
+                class="ml-3"
+              >
+                <v-col align="center">
+                  <template v-if="focusInProgress && !focusFailed">
+                    <v-progress-circular
+                      indeterminate
+                      :size="70"
+                      :width="8"
+                      color="accent"
+                    />
+                    <v-card-text>Camera is being autofocused...</v-card-text>
+                  </template>
+                  <template v-else-if="!focusFailed">
+                    <v-icon
+                      color="green"
+                      size="70"
+                    >
+                      mdi-check-bold
+                    </v-icon>
+                    <v-card-text>Camera has been successfully focused!</v-card-text>
+                  </template>
+                  <template v-else>
+                    <v-icon
+                      color="red"
+                      size="70"
+                    >
+                      mdi-close
+                    </v-icon>
+                    <v-card-text>Camera focus failed! This is likely due to your camera not supporting autofocus. More information is available in the program logs.</v-card-text>
+                  </template>
+                </v-col>
+              </div>
+              <v-card-actions>
+                <v-spacer />
+                <v-btn
+                  v-if="!calibrationInProgress || calibrationFailed"
+                  color="white"
+                  text
+                  @click="closeDialog"
+                >
+                  OK
+                </v-btn>
+              </v-card-actions>
+            </v-card>
+          </v-dialog>
+        </template>
+      </v-col>
     </v-row>
 
     <!-- Special hidden upload input that gets 'clicked' when the user imports settings -->
@@ -427,9 +511,13 @@ export default {
     },
     data() {
         return {
-            snack: false,
+            calibrationSnack: false,
+            focusSnack: false,
             calibrationInProgress: false,
             calibrationFailed: false,
+            focusInProgress: false,
+            focusFailed: false,
+            focusQueryInterval: null,
             filteredVideomodeIndex: 0,
             settingsValid: true,
             unfilteredStreamDivisors: [1, 2, 4],
@@ -655,9 +743,15 @@ export default {
     },
 
         closeDialog() {
-            this.snack = false;
+            this.calibrationSnack = false;
+            this.focusSnack = false;
+
+            this.focusInProgress = false;
+            this.focusFailed = false;
             this.calibrationInProgress = false;
             this.calibrationFailed = false;
+
+           clearInterval(this.focusQueryInterval);
         },
         getCalibrationCoeffs(resolution) {
             const calList = this.$store.getters.calibrationList;
@@ -762,6 +856,44 @@ export default {
             doc.save("calibrationTarget.pdf");
 
         },
+        focusCamera() {
+          this.focusSnack = true;
+          this.focusFailed = false;
+          this.focusInProgress = true;
+
+          this.axios.post("http://" + this.$address + "/api/autofocusCamera", {
+                "index": this.$store.state.currentCameraIndex
+          }).then(()=> {
+              // check every 500ms
+                this.focusQueryInterval = setInterval(()=>this.checkFocusStatus(), 500) 
+              }
+            ).catch(res=>{
+              this.focusFailed = true;
+              clearInterval(this.focusQueryInterval);
+              console.error(res)
+            })
+        },
+        checkFocusStatus() {
+          console.log("Checking focus status...");
+          this.axios.post("http://" + this.$address + "/api/getAutofocusStatus", {
+                "index": this.$store.state.currentCameraIndex
+          }).then(response => {
+              if(response == null || response.data == null || response.data == -1 || response.data == 0 || response.data == 3) {
+                this.focusFailed = true;
+                clearInterval(this.focusQueryInterval);
+                console.error(response)
+              } else if (response.data == 2) {
+                this.focusFailed = false;
+                clearInterval(this.focusQueryInterval);
+                this.focusInProgress = false;
+              }
+              }
+            ).catch(res=>{
+              this.focusFailed = true;
+              clearInterval(this.focusQueryInterval);
+              console.error(res)
+            })
+        },
         sendCameraSettings() {
             this.axios.post("http://" + this.$address + "/api/settings/camera", {
                 "settings": this.cameraSettings,
@@ -800,7 +932,7 @@ export default {
         sendCalibrationFinish() {
             console.log("finishing calibration for index " + this.$store.getters.currentCameraIndex);
 
-            this.snack = true;
+            this.calibrationSnack = true;
             this.calibrationInProgress = true;
 
             this.axios.post("http://" + this.$address + "/api/settings/endCalibration", this.$store.getters.currentCameraIndex)

photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java

@@ -140,6 +140,12 @@ public static native boolean setThresholds(
 
     // Exposure time, in microseconds
     public static native boolean setExposure(int exposureUs);
+
+    // Return int of autofocus status from libcamera AfState
+    public static native int getAutofocusStatus();
+
+    // Set whether or not to use autofocus 
+    public static native boolean setAutofocus(boolean doAutoFocus);
 
     // Set brighness on [-1, 1]
     public static native boolean setBrightness(double brightness);

photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java

@@ -187,6 +187,16 @@ public void setAwbGain(int red, int blue) {
         }
     }
 
+    @Override 
+    public int getAutofocusStatus() {
+      return LibCameraJNI.getAutofocusStatus();
+    }
+
+    @Override
+    public void autofocus() {
+      LibCameraJNI.setAutofocus(true);
+    }
+
     @Override
     public FPSRatedVideoMode getCurrentVideoMode() {
         return currentVideoMode;

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java

@@ -42,6 +42,7 @@ public class CVPipelineSettings implements Cloneable {
     public boolean cameraAutoExposure = false;
     // manual exposure only used if cameraAutoExposure if false
     public double cameraExposure = 20;
+    public boolean cameraAutoFocus = false;
     public int cameraBrightness = 50;
     // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain
     // quirk

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -478,6 +478,14 @@ public void setCameraNickname(String newName) {
         saveAndBroadcastAll();
     }
 
+    public int getAutofocusStatus() {
+      return visionSource.getSettables().getAutofocusStatus();
+    }
+
+    public void autofocus() {
+      visionSource.getSettables().autofocus();
+    }
+
     public PhotonConfiguration.UICameraConfiguration toUICameraConfig() {
         var ret = new PhotonConfiguration.UICameraConfiguration();
 

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java

@@ -45,7 +45,7 @@ public CameraConfiguration getConfiguration() {
     public abstract void setExposure(double exposure);
 
     public abstract void setAutoExposure(boolean cameraAutoExposure);
-
+    
     public abstract void setBrightness(int brightness);
 
     public abstract void setGain(int gain);
@@ -57,6 +57,10 @@ public void setBlueGain(int blue) {}
 
     public abstract VideoMode getCurrentVideoMode();
 
+    public void autofocus(){}
+
+    public int getAutofocusStatus(){return -1;}
+
     public void setVideoModeInternal(int index) {
         setVideoMode(getAllVideoModes().get(index));
     }

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -97,6 +97,9 @@ public HashMap<Integer, VideoMode> getAllVideoModes() {
 
         @Override
         public void setAutoExposure(boolean cameraAutoExposure) {}
+
+        @Override
+        public void setAutoFocus(boolean cameraAutoFocus) {}
     }
 
     private static class TestDataConsumer implements CVPipelineResultConsumer {

photon-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -317,6 +317,35 @@ public static void importCalibrationFromCalibdb(Context ctx) {
         }
     }
 
+    public static void autofocusCamera(Context ctx) {
+        try {
+            var data = kObjectMapper.readValue(ctx.body(), HashMap.class);
+            int idx = Integer.parseInt(String.valueOf(data.get("index")));
+            VisionModuleManager.getInstance().getModule(idx).autofocus();
+            ctx.status(200);
+            return;
+        } catch (Exception e) {
+          logger.warn("Failed to start autofocus!");  
+          e.printStackTrace();
+        }
+        ctx.status(500);
+    }
+
+    public static void getAutofocusStatus(Context ctx) {
+        try {
+            var data = kObjectMapper.readValue(ctx.body(), HashMap.class);
+            int idx = Integer.parseInt(String.valueOf(data.get("index")));
+            int status = VisionModuleManager.getInstance().getModule(idx).getAutofocusStatus();
+            ctx.result(String.valueOf(status));
+            ctx.status(200);
+            return;
+        } catch (Exception e) {
+          logger.warn("Could not check if camera supports autofocus!");  
+          e.printStackTrace();
+        }
+        ctx.status(500);
+    }
+
     public static void setCameraNickname(Context ctx) {
         try {
             var data = kObjectMapper.readValue(ctx.body(), HashMap.class);