Source code

001package edu.wpi.first.wpilibj.vision;
002
003import java.nio.ByteBuffer;
004import java.util.regex.Matcher;
005import java.util.regex.Pattern;
006
007import com.ni.vision.NIVision;
008import com.ni.vision.VisionException;
009
010import static com.ni.vision.NIVision.Image;
011import static edu.wpi.first.wpilibj.Timer.delay;
012
013public class USBCamera {
014        public static String kDefaultCameraName = "cam0";
015
016        private static String ATTR_VIDEO_MODE = "AcquisitionAttributes::VideoMode";
017        private static String ATTR_WB_MODE = "CameraAttributes::WhiteBalance::Mode";
018        private static String ATTR_WB_VALUE = "CameraAttributes::WhiteBalance::Value";
019        private static String ATTR_EX_MODE = "CameraAttributes::Exposure::Mode";
020        private static String ATTR_EX_VALUE = "CameraAttributes::Exposure::Value";
021        private static String ATTR_BR_MODE = "CameraAttributes::Brightness::Mode";
022        private static String ATTR_BR_VALUE = "CameraAttributes::Brightness::Value";
023
024        public class WhiteBalance {
025                public static final int kFixedIndoor = 3000;
026                public static final int kFixedOutdoor1 = 4000;
027                public static final int kFixedOutdoor2 = 5000;
028                public static final int kFixedFluorescent1 = 5100;
029                public static final int kFixedFlourescent2 = 5200;
030        }
031
032        private Pattern m_reMode = Pattern.compile("(?<width>[0-9]+)\\s*x\\s*(?<height>[0-9]+)\\s+(?<format>.*?)\\s+(?<fps>[0-9.]+)\\s*fps");
033
034        private String m_name = kDefaultCameraName;
035        private int m_id = -1;
036        private boolean m_active = false;
037        private boolean m_useJpeg = true;
038        private int m_width = 320;
039        private int m_height = 240;
040        private int m_fps = 30;
041        private String m_whiteBalance = "auto";
042        private int m_whiteBalanceValue = -1;
043        private String m_exposure = "auto";
044        private int m_exposureValue = -1;
045        private int m_brightness = 50;
046        private boolean m_needSettingsUpdate = true;
047
048        public USBCamera() {
049                openCamera();
050        }
051
052        public USBCamera(String name) {
053                m_name = name;
054                openCamera();
055        }
056
057        public synchronized void openCamera() {
058                if (m_id != -1) return; // Camera is already open
059                for (int i=0; i<3; i++) {
060                        try {
061                                m_id = NIVision.IMAQdxOpenCamera(m_name,
062                                        NIVision.IMAQdxCameraControlMode.CameraControlModeController);
063                        } catch (VisionException e) {
064                                if (i == 2)
065                                        throw e;
066                                delay(2.0);
067                                continue;
068                        }
069                        break;
070                }
071        }
072
073        public synchronized void closeCamera() {
074                if (m_id == -1)
075                        return;
076                NIVision.IMAQdxCloseCamera(m_id);
077                m_id = -1;
078        }
079
080        public synchronized void startCapture() {
081                if (m_id == -1 || m_active)
082                        return;
083                NIVision.IMAQdxConfigureGrab(m_id);
084                NIVision.IMAQdxStartAcquisition(m_id);
085                m_active = true;
086        }
087
088        public synchronized void stopCapture() {
089                if (m_id == -1 || !m_active)
090                        return;
091                NIVision.IMAQdxStopAcquisition(m_id);
092                NIVision.IMAQdxUnconfigureAcquisition(m_id);
093                m_active = false;
094        }
095
096        public synchronized void updateSettings() {
097                boolean wasActive = m_active;
098                // Stop acquistion, close and reopen camera
099                if (wasActive)
100                        stopCapture();
101                if (m_id != -1)
102                        closeCamera();
103                openCamera();
104
105                // Video Mode
106                NIVision.dxEnumerateVideoModesResult enumerated = NIVision.IMAQdxEnumerateVideoModes(m_id);
107                NIVision.IMAQdxEnumItem foundMode = null;
108                int foundFps = 1000;
109                for (NIVision.IMAQdxEnumItem mode : enumerated.videoModeArray) {
110                        Matcher m = m_reMode.matcher(mode.Name);
111                        if (!m.matches())
112                                continue;
113                        if (Integer.parseInt(m.group("width")) != m_width)
114                                continue;
115                        if (Integer.parseInt(m.group("height")) != m_height)
116                                continue;
117                        double fps = Double.parseDouble(m.group("fps"));
118                        if (fps < m_fps)
119                                continue;
120                        if (fps > foundFps)
121                                continue;
122                        String format = m.group("format");
123                        boolean isJpeg = format.equals("jpeg") || format.equals("JPEG");
124                        if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg))
125                                continue;
126                        foundMode = mode;
127                        foundFps = (int)fps;
128                }
129                if (foundMode != null) {
130                        System.out.println("found mode " + foundMode.Value + ": " + foundMode.Name);
131                        if (foundMode.Value != enumerated.currentMode)
132                                NIVision.IMAQdxSetAttributeU32(m_id, ATTR_VIDEO_MODE, foundMode.Value);
133                }
134
135                // White Balance
136                if (m_whiteBalance == "auto")
137                        NIVision.IMAQdxSetAttributeString(m_id, ATTR_WB_MODE, "Auto");
138                else {
139                        NIVision.IMAQdxSetAttributeString(m_id, ATTR_WB_MODE, "Manual");
140                        if (m_whiteBalanceValue != -1)
141                                NIVision.IMAQdxSetAttributeI64(m_id, ATTR_WB_VALUE, m_whiteBalanceValue);
142                }
143
144                // Exposure
145                if (m_exposure == "auto")
146                        NIVision.IMAQdxSetAttributeString(m_id, ATTR_EX_MODE, "AutoAperaturePriority");
147                else {
148                        NIVision.IMAQdxSetAttributeString(m_id, ATTR_EX_MODE, "Manual");
149                        if (m_exposureValue != -1) {
150                                long minv = NIVision.IMAQdxGetAttributeMinimumI64(m_id, ATTR_EX_VALUE);
151                                long maxv = NIVision.IMAQdxGetAttributeMaximumI64(m_id, ATTR_EX_VALUE);
152                                long val = minv + (long)(((double)(maxv - minv)) * (((double)m_exposureValue) / 100.0));
153                                NIVision.IMAQdxSetAttributeI64(m_id, ATTR_EX_VALUE, val);
154                        }
155                }
156
157                // Brightness
158                NIVision.IMAQdxSetAttributeString(m_id, ATTR_BR_MODE, "Manual");
159                long minv = NIVision.IMAQdxGetAttributeMinimumI64(m_id, ATTR_BR_VALUE);
160                long maxv = NIVision.IMAQdxGetAttributeMaximumI64(m_id, ATTR_BR_VALUE);
161                long val = minv + (long)(((double)(maxv - minv)) * (((double)m_brightness) / 100.0));
162                NIVision.IMAQdxSetAttributeI64(m_id, ATTR_BR_VALUE, val);
163
164                // Restart acquisition
165                if (wasActive)
166                        startCapture();
167        }
168
169        public synchronized void setFPS(int fps) {
170                if (fps != m_fps) {
171                        m_needSettingsUpdate = true;
172                        m_fps = fps;
173                }
174        }
175
176        public synchronized void setSize(int width, int height) {
177                if (width != m_width || height != m_height) {
178                        m_needSettingsUpdate = true;
179                        m_width = width;
180                        m_height = height;
181                }
182        }
183
184        /** Set the brightness, as a percentage (0-100). */
185        public synchronized void setBrightness(int brightness) {
186                if (brightness > 100)
187                        m_brightness = 100;
188                else if (brightness < 0)
189                        m_brightness = 0;
190                else
191                        m_brightness = brightness;
192                m_needSettingsUpdate = true;
193        }
194
195        /** Get the brightness, as a percentage (0-100). */
196        public synchronized int getBrightness() {
197                return m_brightness;
198        }
199
200        /** Set the white balance to auto. */
201        public synchronized void setWhiteBalanceAuto() {
202                m_whiteBalance = "auto";
203                m_whiteBalanceValue = -1;
204                m_needSettingsUpdate = true;
205        }
206
207        /** Set the white balance to hold current. */
208        public synchronized void setWhiteBalanceHoldCurrent() {
209                m_whiteBalance = "manual";
210                m_whiteBalanceValue = -1;
211                m_needSettingsUpdate = true;
212        }
213
214        /** Set the white balance to manual, with specified color temperature. */
215        public synchronized void setWhiteBalanceManual(int value) {
216                m_whiteBalance = "manual";
217                m_whiteBalanceValue = value;
218                m_needSettingsUpdate = true;
219        }
220
221        /** Set the exposure to auto aperature. */
222        public synchronized void setExposureAuto() {
223                m_exposure = "auto";
224                m_exposureValue = -1;
225                m_needSettingsUpdate = true;
226        }
227
228        /** Set the exposure to hold current. */
229        public synchronized void setExposureHoldCurrent() {
230                m_exposure = "manual";
231                m_exposureValue = -1;
232                m_needSettingsUpdate = true;
233        }
234
235        /** Set the exposure to manual, as a percentage (0-100). */
236        public synchronized void setExposureManual(int value) {
237                m_exposure = "manual";
238                if (value > 100)
239                        m_exposureValue = 100;
240                else if (value < 0)
241                        m_exposureValue = 0;
242                else
243                        m_exposureValue = value;
244                m_needSettingsUpdate = true;
245        }
246
247        public synchronized void getImage(Image image) {
248                if (m_needSettingsUpdate || m_useJpeg) {
249                        m_needSettingsUpdate = false;
250                        m_useJpeg = false;
251                        updateSettings();
252                }
253
254                NIVision.IMAQdxGrab(m_id, image, 1);
255        }
256
257        public synchronized void getImageData(ByteBuffer data) {
258                if (m_needSettingsUpdate || !m_useJpeg) {
259                        m_needSettingsUpdate = false;
260                        m_useJpeg = true;
261                        updateSettings();
262                }
263
264                NIVision.IMAQdxGetImageData(m_id, data, NIVision.IMAQdxBufferNumberMode.BufferNumberModeLast, 0);
265                data.limit(data.capacity() - 1);
266                data.limit(getJpegSize(data));
267        }
268
269        private static int getJpegSize(ByteBuffer data) {
270                if (data.get(0) != (byte) 0xff || data.get(1) != (byte) 0xd8)
271                        throw new VisionException("invalid image");
272                int pos = 2;
273                while (true) {
274                        try {
275                                byte b = data.get(pos);
276                                if (b != (byte) 0xff)
277                                        throw new VisionException("invalid image at pos " + pos + " (" + data.get(pos) + ")");
278                                b = data.get(pos+1);
279                                if (b == (byte) 0x01 || (b >= (byte) 0xd0 && b <= (byte) 0xd7)) // various
280                                        pos += 2;
281                                else if (b == (byte) 0xd9) // EOI
282                                        return pos + 2;
283                                else if (b == (byte) 0xd8) // SOI
284                                        throw new VisionException("invalid image");
285                                else if (b == (byte) 0xda) { // SOS
286                                        int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff);
287                                        pos += len + 2;
288                                        // Find next marker.  Skip over escaped and RST markers.
289                                        while (data.get(pos) != (byte) 0xff || data.get(pos+1) == (byte) 0x00 || (data.get(pos+1) >= (byte) 0xd0 && data.get(pos+1) <= (byte) 0xd7))
290                                                pos += 1;
291                                } else { // various
292                                        int len = ((data.get(pos+2) & 0xff) << 8) | (data.get(pos+3) & 0xff);
293                                        pos += len + 2;
294                                }
295                        } catch (IndexOutOfBoundsException ex) {
296                                throw new VisionException("invalid image: could not find jpeg end " + ex.getMessage());
297                        }
298                }
299        }
300}