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.consumer;
019
020import edu.wpi.first.cscore.*;
021import edu.wpi.first.networktables.NetworkTable;
022import edu.wpi.first.networktables.NetworkTableInstance;
023import edu.wpi.first.util.PixelFormat;
024import java.util.ArrayList;
025import org.photonvision.common.util.math.MathUtils;
026import org.photonvision.vision.frame.StaticFrames;
027import org.photonvision.vision.opencv.CVMat;
028
029public class MJPGFrameConsumer implements AutoCloseable {
030    private static final double MAX_FRAMERATE = -1;
031    private static final long MAX_FRAME_PERIOD_NS = Math.round(1e9 / MAX_FRAMERATE);
032
033    private long lastFrameTimeNs;
034    private CvSource cvSource;
035    private MjpegServer mjpegServer;
036
037    private VideoListener listener;
038
039    private final NetworkTable table;
040
041    public MJPGFrameConsumer(String sourceName, int width, int height, int port) {
042        this.cvSource = new CvSource(sourceName, PixelFormat.kMJPEG, width, height, 30);
043        this.table =
044                NetworkTableInstance.getDefault().getTable("/CameraPublisher").getSubTable(sourceName);
045
046        this.mjpegServer = new MjpegServer("serve_" + cvSource.getName(), port);
047        mjpegServer.setSource(cvSource);
048        mjpegServer.setCompression(75);
049
050        listener =
051                new VideoListener(
052                        event -> {
053                            if (event.kind == VideoEvent.Kind.kNetworkInterfacesChanged) {
054                                table.getEntry("source").setString("cv:");
055                                table.getEntry("streams");
056                                table.getEntry("connected").setBoolean(true);
057                                table.getEntry("mode").setString(videoModeToString(cvSource.getVideoMode()));
058                                table.getEntry("modes").setStringArray(getSourceModeValues(cvSource.getHandle()));
059                                updateStreamValues();
060                            }
061                        },
062                        0x4fff,
063                        true);
064    }
065
066    private synchronized void updateStreamValues() {
067        // Get port
068        int port = mjpegServer.getPort();
069
070        // Generate values
071        var addresses = CameraServerJNI.getNetworkInterfaces();
072        ArrayList<String> values = new ArrayList<>(addresses.length + 1);
073        String listenAddress = CameraServerJNI.getMjpegServerListenAddress(mjpegServer.getHandle());
074        if (!listenAddress.isEmpty()) {
075            // If a listen address is specified, only use that
076            values.add(makeStreamValue(listenAddress, port));
077        } else {
078            // Otherwise generate for hostname and all interface addresses
079            values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
080            for (String addr : addresses) {
081                if ("127.0.0.1".equals(addr)) {
082                    continue; // ignore localhost
083                }
084                values.add(makeStreamValue(addr, port));
085            }
086        }
087
088        String[] streamAddresses = values.toArray(new String[0]);
089        table.getEntry("streams").setStringArray(streamAddresses);
090    }
091
092    public MJPGFrameConsumer(String name, int port) {
093        this(name, 320, 240, port);
094    }
095
096    public void accept(CVMat image) {
097        long now = MathUtils.wpiNanoTime();
098
099        if (image == null || image.getMat() == null || image.getMat().empty()) {
100            image.copyFrom(StaticFrames.LOST_MAT);
101        }
102
103        if (now - lastFrameTimeNs > MAX_FRAME_PERIOD_NS) {
104            lastFrameTimeNs = now;
105            cvSource.putFrame(image.getMat());
106        }
107    }
108
109    public int getCurrentStreamPort() {
110        return mjpegServer.getPort();
111    }
112
113    private static String makeStreamValue(String address, int port) {
114        return "mjpg:http://" + address + ":" + port + "/?action=stream";
115    }
116
117    private static String[] getSourceModeValues(int sourceHandle) {
118        VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
119        String[] modeStrings = new String[modes.length];
120        for (int i = 0; i < modes.length; i++) {
121            modeStrings[i] = videoModeToString(modes[i]);
122        }
123        return modeStrings;
124    }
125
126    private static String videoModeToString(VideoMode mode) {
127        return mode.width
128                + "x"
129                + mode.height
130                + " "
131                + pixelFormatToString(mode.pixelFormat)
132                + " "
133                + mode.fps
134                + " fps";
135    }
136
137    private static String pixelFormatToString(PixelFormat pixelFormat) {
138        return switch (pixelFormat) {
139            case kMJPEG -> "MJPEG";
140            case kYUYV -> "YUYV";
141            case kRGB565 -> "RGB565";
142            case kBGR -> "BGR";
143            case kGray -> "Gray";
144            case kUYVY, kUnknown, kY16, kBGRA -> "Unknown";
145        };
146    }
147
148    @Override
149    public void close() {
150        table.getEntry("connected").setBoolean(false);
151        mjpegServer.close();
152        cvSource.close();
153        listener.close();
154        mjpegServer = null;
155        cvSource = null;
156        listener = null;
157    }
158}