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.pipeline;
019
020import java.util.List;
021import org.apache.commons.lang3.tuple.Pair;
022import org.photonvision.common.util.math.MathUtils;
023import org.photonvision.vision.frame.Frame;
024import org.photonvision.vision.frame.FrameThresholdType;
025import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
026import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
027import org.photonvision.vision.pipe.impl.ResizeImagePipe;
028import org.photonvision.vision.pipeline.result.DriverModePipelineResult;
029
030public class DriverModePipeline
031        extends CVPipeline<DriverModePipelineResult, DriverModePipelineSettings> {
032    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
033    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
034    private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
035
036    private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
037
038    public DriverModePipeline() {
039        super(PROCESSING_TYPE);
040        settings = new DriverModePipelineSettings();
041    }
042
043    public DriverModePipeline(DriverModePipelineSettings settings) {
044        super(PROCESSING_TYPE);
045        this.settings = settings;
046    }
047
048    @Override
049    protected void setPipeParamsImpl() {
050        Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
051                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
052                        frameStaticProperties, settings.streamingFrameDivisor, settings.inputImageRotationMode);
053        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
054
055        resizeImagePipe.setParams(
056                new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
057    }
058
059    @Override
060    public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) {
061        long totalNanos = 0;
062
063        // apply pipes
064        var inputMat = frame.colorImage.getMat();
065
066        boolean emptyIn = inputMat.empty();
067
068        if (!emptyIn) {
069            totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
070
071            if (settings.crosshair) {
072                var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
073
074                // calculate elapsed nanoseconds
075                totalNanos += draw2dCrosshairResult.nanosElapsed;
076            }
077        }
078
079        var fpsResult = calculateFPSPipe.run(null);
080        var fps = fpsResult.output;
081
082        // Flip-flop input and output in the Frame
083        return new DriverModePipelineResult(
084                frame.sequenceID,
085                MathUtils.nanosToMillis(totalNanos),
086                fps,
087                new Frame(
088                        frame.sequenceID,
089                        frame.processedImage,
090                        frame.colorImage,
091                        frame.type,
092                        frame.frameStaticProperties));
093    }
094
095    @Override
096    public void release() {
097        // we never actually need to give resources up since pipelinemanager only makes
098        // one of us
099    }
100}