001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.vision.frame.provider;
019
020import org.opencv.core.Mat;
021import org.photonvision.common.logging.LogGroup;
022import org.photonvision.common.logging.Logger;
023import org.photonvision.common.util.math.MathUtils;
024import org.photonvision.raspi.LibCameraJNI;
025import org.photonvision.vision.camera.csi.LibcameraGpuSettables;
026import org.photonvision.vision.frame.Frame;
027import org.photonvision.vision.frame.FrameProvider;
028import org.photonvision.vision.frame.FrameThresholdType;
029import org.photonvision.vision.opencv.CVMat;
030import org.photonvision.vision.opencv.ImageRotationMode;
031import org.photonvision.vision.pipe.impl.HSVPipe.HSVParams;
032
033public class LibcameraGpuFrameProvider extends FrameProvider {
034    private final LibcameraGpuSettables settables;
035
036    static final Logger logger = new Logger(LibcameraGpuFrameProvider.class, LogGroup.Camera);
037
038    public LibcameraGpuFrameProvider(LibcameraGpuSettables visionSettables) {
039        this.settables = visionSettables;
040
041        var vidMode = settables.getCurrentVideoMode();
042        settables.setVideoMode(vidMode);
043        this.cameraPropertiesCached =
044                true; // Camera properties are not able to be changed so they are always cached
045    }
046
047    @Override
048    public String getName() {
049        return "AcceleratedPicamFrameProvider";
050    }
051
052    int badFrameCounter = 0;
053
054    @Override
055    public Frame get() {
056        // We need to make sure that other threads don't try to change video modes while
057        // we're waiting for a frame
058        // System.out.println("GET!");
059        synchronized (settables.CAMERA_LOCK) {
060            var p_ptr = LibCameraJNI.awaitNewFrame(settables.r_ptr);
061
062            if (p_ptr == 0) {
063                logger.error("No new frame from " + settables.getConfiguration().nickname);
064                badFrameCounter++;
065                if (badFrameCounter > 3) {
066                    logger.error(
067                            "No new frame from "
068                                    + settables.getConfiguration().nickname
069                                    + " for 3 seconds attempting recreate!");
070                    settables.setVideoMode(settables.getCurrentVideoMode());
071                    badFrameCounter = 0;
072                }
073                return new Frame();
074            }
075            badFrameCounter = 0;
076
077            var colorMat = new CVMat(new Mat(LibCameraJNI.takeColorFrame(p_ptr)));
078            var processedMat = new CVMat(new Mat(LibCameraJNI.takeProcessedFrame(p_ptr)));
079
080            // System.out.println("Color mat: " + colorMat.getMat().size());
081
082            // Imgcodecs.imwrite("color" + i + ".jpg", colorMat.getMat());
083            // Imgcodecs.imwrite("processed" + (i) + ".jpg", processedMat.getMat());
084
085            int itype = LibCameraJNI.getGpuProcessType(p_ptr);
086            FrameThresholdType type = FrameThresholdType.NONE;
087            if (itype < FrameThresholdType.values().length && itype >= 0) {
088                type = FrameThresholdType.values()[itype];
089            }
090
091            var now = LibCameraJNI.getLibcameraTimestamp();
092            var capture = LibCameraJNI.getFrameCaptureTime(p_ptr);
093            var latency = (now - capture);
094
095            LibCameraJNI.releasePair(p_ptr);
096
097            // Know frame is good -- increment sequence
098            ++sequenceID;
099
100            return new Frame(
101                    sequenceID,
102                    colorMat,
103                    processedMat,
104                    type,
105                    MathUtils.wpiNanoTime() - latency,
106                    settables.getFrameStaticProperties().rotate(settables.getRotation()));
107        }
108    }
109
110    @Override
111    public void requestFrameThresholdType(FrameThresholdType type) {
112        LibCameraJNI.setGpuProcessType(settables.r_ptr, type.ordinal());
113    }
114
115    @Override
116    public void requestFrameRotation(ImageRotationMode rotationMode) {
117        this.settables.setRotation(rotationMode);
118    }
119
120    @Override
121    public void requestHsvSettings(HSVParams params) {
122        LibCameraJNI.setThresholds(
123                settables.r_ptr,
124                params.getHsvLower().val[0] / 180.0,
125                params.getHsvLower().val[1] / 255.0,
126                params.getHsvLower().val[2] / 255.0,
127                params.getHsvUpper().val[0] / 180.0,
128                params.getHsvUpper().val[1] / 255.0,
129                params.getHsvUpper().val[2] / 255.0,
130                params.getHueInverted());
131    }
132
133    @Override
134    public void requestFrameCopies(boolean copyInput, boolean copyOutput) {
135        LibCameraJNI.setFramesToCopy(settables.r_ptr, copyInput, copyOutput);
136    }
137
138    @Override
139    public void release() {
140        synchronized (settables.CAMERA_LOCK) {
141            LibCameraJNI.stopCamera(settables.r_ptr);
142            LibCameraJNI.destroyCamera(settables.r_ptr);
143            settables.r_ptr = 0;
144        }
145    }
146
147    @Override
148    public boolean checkCameraConnected() {
149        String[] cameraNames = LibCameraJNI.getCameraNames();
150        for (String name : cameraNames) {
151            if (name.equals(settables.getConfiguration().getDevicePath())) {
152                return true;
153            }
154        }
155        return false;
156    }
157
158    // To our knowledge the camera is always connected (after boot) with csi cameras
159    @Override
160    public boolean isConnected() {
161        return checkCameraConnected();
162    }
163}