Add support for autofocus

[jackpittenger]

2023-01-21.18-41-46.mp4

This PR intends to add support for autofocus with Photonvision. I am using Arducam Picam V3 to test. See the attached video to see it working. I set up the code very similar to auto exposure, but I would appreciate someone looking to see if I missed anything.

The respective PR for integration into PV is here

I haven't found a good way to determine if the camera being used supports AF. If it doesn't, the following continuously prints:
WARN IPARPI raspberrypi.cpp:796 Could not set AF_MODE - no AF algorithm
Two options I see:

1. Attempt to set auto exposure. Check libcamera::controls::AfState. If it fails, temporarily disable autofocus.
2. Copy the code that checks for this from raspberrypi.cpp

+			RPiController::AfAlgorithm *af = dynamic_cast<RPiController::AfAlgorithm *>(
+				controller_.GetAlgorithm("iob.af"));
+			if (!af) {
+				...

Thoughts?

[mcm001]

You should check the camera model (that's some code I wrote), and only set auto focus if it's a V3? Also, per an earlier Discord conversation, autofocus focusing will mess up camera calibration coefficients -- maybe it makes the most sense to just lock it at infinity?

[mcm001]

Also can we undo all the reformatting this PR does? I can't tell what I should be reviewing

[jackpittenger]

Sorry, I may not have been clear as to the purpose. The end goal is to have a button on the camera calibration page to focus the camera for use prior to calibrating the camera. It is not intended to focus the camera during use.
Flow:

1. focus camera button -->warn users this will delete their previous calibrations
2. Continuous focus enabled
3. stop focus button ends the focus routine and leaves a well focused camera

I used the included format script for my changes but the rest of the repo didn't seem to have been run through it. I don't see an easy way to undo the formatting of other areas while keeping the formatting of my changes. Important changes are in camera_grabber.h/cpp.

[jackpittenger]

You should check the camera model (that's some code I wrote), and only set auto focus if it's a V3?

What if they don't have the AF compatibility installed (very possible) despite using the V3?