diff --git a/.gitignore b/.gitignore index ac616f55..9e982557 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,9 @@ # production /build +# media storage +/media + # misc .DS_Store .env @@ -28,4 +31,4 @@ settingsTest.json /flightlogs .nyc_output -/local_modules/long/node_modules \ No newline at end of file +/local_modules/long/node_modules diff --git a/package-lock.json b/package-lock.json index 7b40360a..07c1bbd1 100644 --- a/package-lock.json +++ b/package-lock.json @@ -58,7 +58,9 @@ "npm-run-all": "^4.1.5", "pino-colada": "^2.2.2", "pino-http": "^10.3.0", - "should": "^13.2.3" + "should": "^13.2.3", + "sinon": "^19.0.2", + "sinon-chai": "^3.7.0" } }, "node_modules/@alloc/quick-lru": { @@ -4008,6 +4010,45 @@ "@sinonjs/commons": "^1.7.0" } }, + "node_modules/@sinonjs/samsam": { + "version": "8.0.2", + "resolved": "https://registry.npmjs.org/@sinonjs/samsam/-/samsam-8.0.2.tgz", + "integrity": "sha512-v46t/fwnhejRSFTGqbpn9u+LQ9xJDse10gNnPgAcxgdoCDMXj/G2asWAC/8Qs+BAZDicX+MNZouXT1A7c83kVw==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "@sinonjs/commons": "^3.0.1", + "lodash.get": "^4.4.2", + "type-detect": "^4.1.0" + } + }, + "node_modules/@sinonjs/samsam/node_modules/@sinonjs/commons": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/@sinonjs/commons/-/commons-3.0.1.tgz", + "integrity": "sha512-K3mCHKQ9sVh8o1C9cxkwxaOmXoAMlDxC1mYyHrjqOWEcBjYr76t96zL2zlj5dUGZ3HSw240X1qgH3Mjf1yJWpQ==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "type-detect": "4.0.8" + } + }, + "node_modules/@sinonjs/samsam/node_modules/@sinonjs/commons/node_modules/type-detect": { + "version": "4.0.8", + "resolved": "https://registry.npmjs.org/type-detect/-/type-detect-4.0.8.tgz", + "integrity": "sha512-0fr/mIH1dlO+x7TlcMy+bIDqKPsw/70tVyeHW787goQjhmqaZe10uwLujubK9q9Lg6Fiho1KUKDYz0Z7k7g5/g==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=4" + } + }, + "node_modules/@sinonjs/text-encoding": { + "version": "0.7.3", + "resolved": "https://registry.npmjs.org/@sinonjs/text-encoding/-/text-encoding-0.7.3.tgz", + "integrity": "sha512-DE427ROAphMQzU4ENbliGYrBSYPXF+TtLg9S8vzeA+OF4ZKzoDdzfL8sxuMUGS/lgRhM6j1URSk9ghf7Xo1tyA==", + "dev": true, + "license": "(Unlicense OR Apache-2.0)" + }, "node_modules/@socket.io/component-emitter": { "version": "3.1.2", "resolved": "https://registry.npmjs.org/@socket.io/component-emitter/-/component-emitter-3.1.2.tgz", @@ -13022,6 +13063,13 @@ "node": ">=4.0" } }, + "node_modules/just-extend": { + "version": "6.2.0", + "resolved": "https://registry.npmjs.org/just-extend/-/just-extend-6.2.0.tgz", + "integrity": "sha512-cYofQu2Xpom82S6qD778jBDpwvvy39s1l/hrYij2u9AMdQcGRpaBu6kY4mVhuno5kJVi1DAz4aiphA2WI1/OAw==", + "dev": true, + "license": "MIT" + }, "node_modules/keyv": { "version": "4.5.4", "resolved": "https://registry.npmjs.org/keyv/-/keyv-4.5.4.tgz", @@ -13235,6 +13283,13 @@ "integrity": "sha512-uHaJFihxmJcEX3kT4I23ABqKKalJ/zDrDg0lsFtc1h+3uw49SIJ5beyhx5ExVRti3AvKoOJngIj7xz3oylPdWQ==", "license": "MIT" }, + "node_modules/lodash.get": { + "version": "4.4.2", + "resolved": "https://registry.npmjs.org/lodash.get/-/lodash.get-4.4.2.tgz", + "integrity": "sha512-z+Uw/vLuy6gQe8cfaFWD7p0wVv8fJl3mbzXh33RS+0oW2wvUqiRXiQ69gLWSLpgB5/6sU+r6BlQR0MBILadqTQ==", + "dev": true, + "license": "MIT" + }, "node_modules/lodash.memoize": { "version": "4.1.2", "resolved": "https://registry.npmjs.org/lodash.memoize/-/lodash.memoize-4.1.2.tgz", @@ -13853,6 +13908,60 @@ "dev": true, "license": "MIT" }, + "node_modules/nise": { + "version": "6.1.1", + "resolved": "https://registry.npmjs.org/nise/-/nise-6.1.1.tgz", + "integrity": "sha512-aMSAzLVY7LyeM60gvBS423nBmIPP+Wy7St7hsb+8/fc1HmeoHJfLO8CKse4u3BtOZvQLJghYPI2i/1WZrEj5/g==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "@sinonjs/commons": "^3.0.1", + "@sinonjs/fake-timers": "^13.0.1", + "@sinonjs/text-encoding": "^0.7.3", + "just-extend": "^6.2.0", + "path-to-regexp": "^8.1.0" + } + }, + "node_modules/nise/node_modules/@sinonjs/commons": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/@sinonjs/commons/-/commons-3.0.1.tgz", + "integrity": "sha512-K3mCHKQ9sVh8o1C9cxkwxaOmXoAMlDxC1mYyHrjqOWEcBjYr76t96zL2zlj5dUGZ3HSw240X1qgH3Mjf1yJWpQ==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "type-detect": "4.0.8" + } + }, + "node_modules/nise/node_modules/@sinonjs/fake-timers": { + "version": "13.0.2", + "resolved": "https://registry.npmjs.org/@sinonjs/fake-timers/-/fake-timers-13.0.2.tgz", + "integrity": "sha512-4Bb+oqXZTSTZ1q27Izly9lv8B9dlV61CROxPiVtywwzv5SnytJqhvYe6FclHYuXml4cd1VHPo1zd5PmTeJozvA==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "@sinonjs/commons": "^3.0.1" + } + }, + "node_modules/nise/node_modules/path-to-regexp": { + "version": "8.2.0", + "resolved": "https://registry.npmjs.org/path-to-regexp/-/path-to-regexp-8.2.0.tgz", + "integrity": "sha512-TdrF7fW9Rphjq4RjrW0Kp2AW0Ahwu9sRGTkS6bvDi0SCwZlEZYmcfDbEsTz8RVk0EHIS/Vd1bv3JhG+1xZuAyQ==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=16" + } + }, + "node_modules/nise/node_modules/type-detect": { + "version": "4.0.8", + "resolved": "https://registry.npmjs.org/type-detect/-/type-detect-4.0.8.tgz", + "integrity": "sha512-0fr/mIH1dlO+x7TlcMy+bIDqKPsw/70tVyeHW787goQjhmqaZe10uwLujubK9q9Lg6Fiho1KUKDYz0Z7k7g5/g==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=4" + } + }, "node_modules/no-case": { "version": "3.0.4", "resolved": "https://registry.npmjs.org/no-case/-/no-case-3.0.4.tgz", @@ -21956,6 +22065,76 @@ "node": ">=10" } }, + "node_modules/sinon": { + "version": "19.0.2", + "resolved": "https://registry.npmjs.org/sinon/-/sinon-19.0.2.tgz", + "integrity": "sha512-euuToqM+PjO4UgXeLETsfQiuoyPXlqFezr6YZDFwHR3t4qaX0fZUe1MfPMznTL5f8BWrVS89KduLdMUsxFCO6g==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "@sinonjs/commons": "^3.0.1", + "@sinonjs/fake-timers": "^13.0.2", + "@sinonjs/samsam": "^8.0.1", + "diff": "^7.0.0", + "nise": "^6.1.1", + "supports-color": "^7.2.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/sinon" + } + }, + "node_modules/sinon-chai": { + "version": "3.7.0", + "resolved": "https://registry.npmjs.org/sinon-chai/-/sinon-chai-3.7.0.tgz", + "integrity": "sha512-mf5NURdUaSdnatJx3uhoBOrY9dtL19fiOtAdT1Azxg3+lNJFiuN0uzaU3xX1LeAfL17kHQhTAJgpsfhbMJMY2g==", + "dev": true, + "license": "(BSD-2-Clause OR WTFPL)", + "peerDependencies": { + "chai": "^4.0.0", + "sinon": ">=4.0.0" + } + }, + "node_modules/sinon/node_modules/@sinonjs/commons": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/@sinonjs/commons/-/commons-3.0.1.tgz", + "integrity": "sha512-K3mCHKQ9sVh8o1C9cxkwxaOmXoAMlDxC1mYyHrjqOWEcBjYr76t96zL2zlj5dUGZ3HSw240X1qgH3Mjf1yJWpQ==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "type-detect": "4.0.8" + } + }, + "node_modules/sinon/node_modules/@sinonjs/fake-timers": { + "version": "13.0.2", + "resolved": "https://registry.npmjs.org/@sinonjs/fake-timers/-/fake-timers-13.0.2.tgz", + "integrity": "sha512-4Bb+oqXZTSTZ1q27Izly9lv8B9dlV61CROxPiVtywwzv5SnytJqhvYe6FclHYuXml4cd1VHPo1zd5PmTeJozvA==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "@sinonjs/commons": "^3.0.1" + } + }, + "node_modules/sinon/node_modules/diff": { + "version": "7.0.0", + "resolved": "https://registry.npmjs.org/diff/-/diff-7.0.0.tgz", + "integrity": "sha512-PJWHUb1RFevKCwaFA9RlG5tCd+FO5iRh9A8HEtkmBH2Li03iJriB6m6JIN4rGz3K3JLawI7/veA1xzRKP6ISBw==", + "dev": true, + "license": "BSD-3-Clause", + "engines": { + "node": ">=0.3.1" + } + }, + "node_modules/sinon/node_modules/type-detect": { + "version": "4.0.8", + "resolved": "https://registry.npmjs.org/type-detect/-/type-detect-4.0.8.tgz", + "integrity": "sha512-0fr/mIH1dlO+x7TlcMy+bIDqKPsw/70tVyeHW787goQjhmqaZe10uwLujubK9q9Lg6Fiho1KUKDYz0Z7k7g5/g==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=4" + } + }, "node_modules/sisteransi": { "version": "1.0.5", "resolved": "https://registry.npmjs.org/sisteransi/-/sisteransi-1.0.5.tgz", diff --git a/package.json b/package.json index 74af8f13..32b522da 100644 --- a/package.json +++ b/package.json @@ -75,7 +75,9 @@ "npm-run-all": "^4.1.5", "pino-colada": "^2.2.2", "pino-http": "^10.3.0", - "should": "^13.2.3" + "should": "^13.2.3", + "sinon": "^19.0.2", + "sinon-chai": "^3.7.0" }, "nyc": { "all": true, diff --git a/python/photomode.py b/python/photomode.py new file mode 100755 index 00000000..fa7b6491 --- /dev/null +++ b/python/photomode.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +# -*- coding:utf-8 vi:ts=4:noexpandtab + +from picamera2 import Picamera2 +from picamera2.encoders import H264Encoder + +import argparse +import time, signal, os + +from gi.repository import GLib + +# Reset the signal flag +GOT_SIGNAL = False +# Get the PID (for testing/troubleshooting) +pid = os.getpid() +print("PID is : ", pid) + +def receive_signal(signum, stack): + global GOT_SIGNAL + GOT_SIGNAL = True + +# Register the signal handler function to fire when signals are received +signal.signal(signal.SIGUSR1, receive_signal) + +# Reset the video recording flag +VIDEO_ACTIVE = False + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Camera control server using libcamera") + parser.add_argument("-d", "--destination", dest="mediaPath", + help="Save captured image to PATH. Default: ../media/", + metavar="PATH", + default="../media/" + ) + parser.add_argument("-m", "--mode", choices=['photo', 'video'], + dest="captureMode", + help="Capture mode options: photo [default], video", metavar="MODE", + default='photo' + ) + parser.add_argument("-b", "--bitrate", metavar = "N", + type = int, dest="vidBitrate", + help="Video bitrate in bits per second. Default: 10000000", + default=10000000 + ) + args = parser.parse_args() + +captureMode = args.captureMode +print("Mode is: ", captureMode) + +mediaPath = args.mediaPath +print("Media storage directory is:", mediaPath) + +if captureMode == "video": + vidBitrate = args.vidBitrate + print("Video Bitrate is: ", vidBitrate, " (", vidBitrate / 1000000, " MBps)", sep = "") + + +# Initialize the camera +if captureMode == "photo": + # Initialize the camera + picam2_still = Picamera2() + # By default, use the full resolution of the sensor + config = picam2_still.create_still_configuration( + main={"size": picam2_still.sensor_resolution}, + buffer_count=2 + ) + picam2_still.configure(config) + # Keep the camera active to make responses faster + picam2_still.start() + print("Waiting 2 seconds for camera to stabilize...") + time.sleep(2) + print("Camera is ready") + +elif captureMode == "video": + picam2_vid = Picamera2() + video_config = picam2_vid.create_video_configuration() + picam2_vid.configure(video_config) + + encoder = H264Encoder(bitrate = vidBitrate) + +def startstop_video(): + global VIDEO_ACTIVE + + if VIDEO_ACTIVE: + picam2_vid.stop_recording() + VIDEO_ACTIVE = False + print ("Video recording stopped.") + else: + filename = time.strftime("RPN%Y%m%d_%H%M%S.h264") + filepath = os.path.join(mediaPath, filename) + print("Recording to ", filepath) + + VIDEO_ACTIVE = True + output_video = picam2_vid.start_recording(encoder, filepath) + +try: + # Wait for a signal to arrive + while True: + if (GOT_SIGNAL and (captureMode == "photo")): + GOT_SIGNAL = False + print("Received signal.SIGUSR1. Capturing photo.") + filename = time.strftime("/home/pi/Rpanion-server/media/RPN%Y%m%d_%H%M%S.jpg") + print(filename) + output_orig = picam2_still.capture_file(filename) + elif (GOT_SIGNAL and (captureMode == "video")): + GOT_SIGNAL = False + print("Received signal.SIGUSR1.") + startstop_video() + + # Wait for a signal + signal.pause() + #loop.run() +except: + print("Exiting Photo/Video Server") \ No newline at end of file diff --git a/server/index.js b/server/index.js index adb9146d..da863bac 100644 --- a/server/index.js +++ b/server/index.js @@ -116,6 +116,43 @@ vManager.eventEmitter.on('videostreaminfo', (msg, senderSysId, senderCompId, tar } }) +// Got a CAMERA_SETTINGS event, send to flight controller +vManager.eventEmitter.on('camerasettings', (msg, senderSysId, senderCompId, targetComponent) => { + try { + if (fcManager.m) { + fcManager.m.sendCommandAck(common.CameraSettings.MSG_ID, 0, senderSysId, senderCompId, targetComponent) + fcManager.m.sendData(msg, senderCompId) + } + } catch (err) { + console.log(err) + } +}) + +// Got a DO_DIGICAM_CONTROL event, send to flight controller +vManager.eventEmitter.on('digicamcontrol', (senderSysId, senderCompId, targetComponent) => { + console.log("index.js:digicamcontrol event received") + try { + if (fcManager.m) { + // 203 = MAV_CMD_DO_DIGICAM_CONTROL + fcManager.m.sendCommandAck(203, 0, senderSysId, senderCompId, targetComponent) + } + } catch (err) { + console.log(err) + } +}) + +// Got a CAMERA_TRIGGER event, send to flight controller +vManager.eventEmitter.on('cameratrigger', (msg, senderSysId, senderCompId, targetComponent) => { + try { + if (fcManager.m) { + fcManager.m.sendCommandAck(common.CameraTrigger.MSG_ID, 0, senderSysId, senderCompId, targetComponent) + fcManager.m.sendData(msg, senderCompId) + } + } catch (err) { + console.log(err) + } +}) + // Connecting the flight controller datastream to the logger // and ntrip and video fcManager.eventEmitter.on('gotMessage', (packet, data) => { @@ -428,7 +465,7 @@ app.get('/api/softwareinfo', (req, res) => { app.get('/api/videodevices', (req, res) => { vManager.populateAddresses() - vManager.getVideoDevices((err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, useCameraHeartbeat, selMavURI) => { + vManager.getVideoDevices((err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SelusePhotoMode, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, useCameraHeartbeat, useMavControl, selMavURI, selMediaPath) => { if (!err) { res.setHeader('Content-Type', 'application/json') res.send(JSON.stringify({ @@ -443,6 +480,7 @@ app.get('/api/videodevices', (req, res) => { bitrate: selbitrate, fpsSelected: selfps, UDPChecked: SeluseUDP, + photoMode: SelusePhotoMode, useUDPIP: SeluseUDPIP, useUDPPort: SeluseUDPPort, timestamp, @@ -450,7 +488,9 @@ app.get('/api/videodevices', (req, res) => { fps: fps, FPSMax: FPSMax, enableCameraHeartbeat: useCameraHeartbeat, - mavStreamSelected: selMavURI + enableMavControl: useMavControl, + mavStreamSelected: selMavURI, + mediaPath: selMediaPath })) } else { res.setHeader('Content-Type', 'application/json') @@ -697,14 +737,17 @@ app.post('/api/startstopvideo', [check('active').isBoolean(), check('height').if(check('active').isIn([true])).isInt({ min: 1 }), check('width').if(check('active').isIn([true])).isInt({ min: 1 }), check('useUDP').if(check('active').isIn([true])).isBoolean(), + check('usePhotoMode').if(check('active').isIn([true])).isBoolean(), check('useTimestamp').if(check('active').isIn([true])).isBoolean(), check('useCameraHeartbeat').if(check('active').isIn([true])).isBoolean(), + check('useMavControl').if(check('active').isIn([true])).isBoolean(), check('useUDPPort').if(check('active').isIn([true])).isPort(), check('useUDPIP').if(check('active').isIn([true])).isIP(), check('bitrate').if(check('active').isIn([true])).isInt({ min: 50, max: 50000 }), check('format').if(check('active').isIn([true])).isIn(['video/x-raw', 'video/x-h264', 'image/jpeg']), check('fps').if(check('active').isIn([true])).isInt({ min: -1, max: 100 }), - check('rotation').if(check('active').isIn([true])).isInt().isIn([0, 90, 180, 270])], (req, res) => { + check('rotation').if(check('active').isIn([true])).isInt().isIn([0, 90, 180, 270])], + check('mediaPath').if(check('active').isIn([true])).isLength({ min: 2 }), (req, res) => { const errors = validationResult(req) if (!errors.isEmpty()) { winston.error('Bad POST vars in /api/startstopvideo ', { message: errors.array() }) @@ -712,7 +755,10 @@ app.post('/api/startstopvideo', [check('active').isBoolean(), return res.status(422).json(ret) } // user wants to start/stop video streaming - vManager.startStopStreaming(req.body.active, req.body.device, req.body.height, req.body.width, req.body.format, req.body.rotation, req.body.bitrate, req.body.fps, req.body.useUDP, req.body.useUDPIP, req.body.useUDPPort, req.body.useTimestamp, req.body.useCameraHeartbeat, req.body.mavStreamSelected, (err, status, addresses) => { + vManager.startStopStreaming(req.body.active, req.body.device, req.body.height, req.body.width, req.body.format, + req.body.rotation, req.body.bitrate, req.body.fps, req.body.useUDP, req.body.usePhotoMode, req.body.useUDPIP, + req.body.useUDPPort, req.body.useTimestamp, req.body.useCameraHeartbeat, req.body.useMavControl, req.body.mavStreamSelected, + req.body.mediaPath, (err, status, addresses) => { if (!err) { res.setHeader('Content-Type', 'application/json') const ret = { streamingStatus: status, streamAddresses: addresses } @@ -726,6 +772,13 @@ app.post('/api/startstopvideo', [check('active').isBoolean(), }) }) +// Capture a single still photo when in photo mode +app.post('/api/capturestillphoto', function (req, res) { + vManager.captureStillPhoto() + console.log(req.body) + res.end(); +}) + // Get details of a network connection by connection ID app.post('/api/networkIP', [check('conName').isUUID()], (req, res) => { // Finds the validation errors in this request and wraps them in an object with handy functions diff --git a/server/videostream.js b/server/videostream.js index 076dd847..90531789 100644 --- a/server/videostream.js +++ b/server/videostream.js @@ -12,6 +12,7 @@ class videoStream { this.devices = null this.settings = settings this.savedDevice = null + this.photoSeq = 0 this.winston = winston @@ -29,12 +30,13 @@ class videoStream { // need to scan for video devices first though if (this.active) { this.active = false - this.getVideoDevices((error, devices, active, seldevice, selRes, selRot, selbitrate, selfps, selUDP, selUDPIP, selUDPPort, useTimestamp, useCameraHeartbeat, selMavURI) => { + this.getVideoDevices((error, devices, active, seldevice, selRes, selRot, selbitrate, selfps, selUDP, selPhotoMode, selUDPIP, selUDPPort, useTimestamp, useCameraHeartbeat, useMavControl, selMavURI, selMediaPath) => { if (!error) { this.startStopStreaming(true, this.savedDevice.device, this.savedDevice.height, this.savedDevice.width, this.savedDevice.format, - this.savedDevice.rotation, this.savedDevice.bitrate, this.savedDevice.fps, this.savedDevice.useUDP, - this.savedDevice.useUDPIP, this.savedDevice.useUDPPort, this.savedDevice.useTimestamp, this.savedDevice.useCameraHeartbeat, this.savedDevice.mavStreamSelected, + this.savedDevice.rotation, this.savedDevice.bitrate, this.savedDevice.fps, this.savedDevice.useUDP, this.savedDevice.usePhotoMode, + this.savedDevice.useUDPIP, this.savedDevice.useUDPPort, this.savedDevice.useTimestamp, this.savedDevice.useCameraHeartbeat, + this.savedDevice.useMavControl, this.savedDevice.mavStreamSelected, this.savedDevice.mediaPath, (err, status, addresses) => { if (err) { // failed setup, reset settings @@ -42,6 +44,7 @@ class videoStream { this.resetVideo() } }) + this.winston.info('Media path is: ', this.savedDevice.mediaPath) } else { // failed setup, reset settings console.log('Reset video3') @@ -65,7 +68,8 @@ class videoStream { // video streaming getVideoDevices (callback) { // get all video device details - // callback is: err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, cameraHeartbeat, selMavURI + // callback is: err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SelusePhotoMode, SeluseUDPIP, + // SeluseUDPPort, timestamp, fps, FPSMax, vidres, cameraHeartbeat, mavControl, selMavURI, selMediaPath exec('python3 ./python/gstcaps.py', (error, stdout, stderr) => { const warnstrings = ['DeprecationWarning', 'gst_element_message_full_with_details', 'camera_manager.cpp', 'Unsupported V4L2 pixel format'] if (stderr && !warnstrings.some(wrn => stderr.includes(wrn))) { @@ -82,9 +86,9 @@ class videoStream { // and return current settings if (!this.active) { return callback(null, this.devices, this.active, this.devices[0], this.devices[0].caps[0], - { label: '0°', value: 0 }, 1100, fpsSelected, false, '127.0.0.1', 5400, false, + { label: '0°', value: 0 }, 1100, fpsSelected, false, false, '127.0.0.1', 5400, false, (this.devices[0].caps[0].fps !== undefined) ? this.devices[0].caps[0].fps : [], - this.devices[0].caps[0].fpsmax, this.devices[0].caps, false, { label: '127.0.0.1', value: 0 }) + this.devices[0].caps[0].fpsmax, this.devices[0].caps, false, false, { label: '127.0.0.1', value: 0 }, '/home/pi/Rpanion-server/media/') } else { // format saved settings const seldevice = this.devices.filter(it => it.value === this.savedDevice.device) @@ -94,9 +98,9 @@ class videoStream { this.winston.error('Bad video settings. Resetting ', { message: this.savedDevice }) this.resetVideo() return callback(null, this.devices, this.active, this.devices[0], this.devices[0].caps[0], - { label: '0°', value: 0 }, 1100, fpsSelected, false, '127.0.0.1', 5400, false, + { label: '0°', value: 0 }, 1100, fpsSelected, false, false, '127.0.0.1', 5400, false, (this.devices[0].caps[0].fps !== undefined) ? this.devices[0].caps[0].fps : [], - this.devices[0].caps[0].fpsmax, this.devices[0].caps, false, { label: '127.0.0.1', value: 0 }) + this.devices[0].caps[0].fpsmax, this.devices[0].caps, false, false, { label: '127.0.0.1', value: 0 }, '/home/pi/Rpanion-server/media/') } const selRes = seldevice[0].caps.filter(it => it.value === this.savedDevice.width.toString() + 'x' + this.savedDevice.height.toString() + 'x' + this.savedDevice.format.toString().split('/')[1]) let selFPS = this.savedDevice.fps @@ -108,18 +112,22 @@ class videoStream { console.log(seldevice[0]) return callback(null, this.devices, this.active, seldevice[0], selRes[0], { label: this.savedDevice.rotation.toString() + '°', value: this.savedDevice.rotation }, - this.savedDevice.bitrate, selFPS, this.savedDevice.useUDP, this.savedDevice.useUDPIP, + this.savedDevice.bitrate, selFPS, this.savedDevice.useUDP, this.savedDevice.usePhotoMode, this.savedDevice.useUDPIP, this.savedDevice.useUDPPort, this.savedDevice.useTimestamp, (selRes[0].fps !== undefined) ? selRes[0].fps : [], - selRes[0].fpsmax, seldevice[0].caps, this.savedDevice.useCameraHeartbeat, { label: this.savedDevice.mavStreamSelected.toString(), value: this.savedDevice.mavStreamSelected }) + selRes[0].fpsmax, seldevice[0].caps, this.savedDevice.useCameraHeartbeat, this.savedDevice.useMavControl, + { label: this.savedDevice.mavStreamSelected.toString(), value: this.savedDevice.mavStreamSelected }, + this.savedDevice.mediaPath + ) } else { // bad settings console.error('Bad video settings. Resetting' + seldevice + ', ' + selRes) this.winston.error('Bad video settings. Resetting ', { message: JSON.stringify(this.savedDevice) }) this.resetVideo() return callback(null, this.devices, this.active, this.devices[0], this.devices[0].caps[0], - { label: '0°', value: 0 }, 1100, fpsSelected, false, '127.0.0.1', 5400, false, + { label: '0°', value: 0 }, 1100, fpsSelected, false, false, '127.0.0.1', 5400, false, (this.devices[0].caps[0].fps !== undefined) ? this.devices[0].caps[0].fps : [], - this.devices[0].caps[0].fpsmax, this.devices[0].caps, false, { label: '127.0.0.1', value: 0 }) + this.devices[0].caps[0].fpsmax, this.devices[0].caps, false, false, { label: '127.0.0.1', value: 0 }, + '/home/pi/Rpanion-server/media/') } } } @@ -164,7 +172,7 @@ class videoStream { return iface } - async startStopStreaming (active, device, height, width, format, rotation, bitrate, fps, useUDP, useUDPIP, useUDPPort, useTimestamp, useCameraHeartbeat, mavStreamSelected, callback) { + async startStopStreaming (active, device, height, width, format, rotation, bitrate, fps, useUDP, usePhotoMode, useUDPIP, useUDPPort, useTimestamp, useCameraHeartbeat, useMavControl, mavStreamSelected, mediaPath, callback) { // if current state same, don't do anything if (this.active === active) { console.log('Video current same') @@ -201,78 +209,89 @@ class videoStream { fps, rotation, useUDP, + usePhotoMode, useUDPIP, useUDPPort, useTimestamp, useCameraHeartbeat, - mavStreamSelected + useMavControl, + mavStreamSelected, + mediaPath } - // note that video device URL's are the alphanumeric characters only. So /dev/video0 -> devvideo0 - this.populateAddresses(device.replace(/\W/g, '')) + this.winston.info('from startStopStreamning(), media path is: ', this.savedDevice.mediaPath) - // rpi camera has different name under Ubuntu - if (await this.isUbuntu() && device === 'rpicam') { - device = '/dev/video0' - format = 'video/x-raw' - } - - const args = ['./python/rtsp-server.py', - '--video=' + device, - '--height=' + height, - '--width=' + width, - '--format=' + format, - '--bitrate=' + bitrate, - '--rotation=' + rotation, - '--fps=' + fps, - '--udp=' + ((useUDP === false) ? '0' : useUDPIP + ':' + useUDPPort.toString()) - ] - - if (useTimestamp) { - args.push('--timestamp') - } + // Don't start a video stream if we are in photo mode + if (this.savedDevice.usePhotoMode) { + console.log('Started photo mode') + } else { + // note that video device URL's are the alphanumeric characters only. So /dev/video0 -> devvideo0 + this.populateAddresses(device.replace(/\W/g, '')) - this.deviceStream = spawn('python3', args) + // rpi camera has different name under Ubuntu + if (await this.isUbuntu() && device === 'rpicam') { + device = '/dev/video0' + format = 'video/x-raw' + } - try { - if (this.deviceStream === null) { - this.settings.setValue('videostream.active', false) - console.log('Error spawning rtsp-server.py') - this.winston.info('Error spawning rtsp-server.py') - return callback(null, this.active, this.deviceAddresses) + const args = ['./python/rtsp-server.py', + '--video=' + device, + '--height=' + height, + '--width=' + width, + '--format=' + format, + '--bitrate=' + bitrate, + '--rotation=' + rotation, + '--fps=' + fps, + '--udp=' + ((useUDP === false) ? '0' : useUDPIP + ':' + useUDPPort.toString()) + ] + + if (useTimestamp) { + args.push('--timestamp') } - this.settings.setValue('videostream.active', this.active) - this.settings.setValue('videostream.savedDevice', this.savedDevice) - } catch (e) { - console.log(e) - this.winston.info(e) - } - this.deviceStream.stdout.on('data', (data) => { - this.winston.info('startStopStreaming() data ' + data) - console.log(`GST stdout: ${data}`) - }) + this.deviceStream = spawn('python3', args) - this.deviceStream.stderr.on('data', (data) => { - this.winston.error('startStopStreaming() err ', { message: data }) - console.error(`GST stderr: ${data}`) - }) + try { + if (this.deviceStream === null) { + this.settings.setValue('videostream.active', false) + console.log('Error spawning rtsp-server.py') + this.winston.info('Error spawning rtsp-server.py') + return callback(null, this.active, this.deviceAddresses) + } + this.settings.setValue('videostream.active', this.active) + this.settings.setValue('videostream.savedDevice', this.savedDevice) + } catch (e) { + console.log(e) + this.winston.info(e) + } - this.deviceStream.on('close', (code) => { - console.log(`GST process exited with code ${code}`) - this.winston.info('startStopStreaming() close ' + code) - this.deviceStream.stdin.pause() - this.deviceStream.kill() - this.resetVideo() - }) + this.deviceStream.stdout.on('data', (data) => { + this.winston.info('startStopStreaming() data ' + data) + console.log(`GST stdout: ${data}`) + }) + + this.deviceStream.stderr.on('data', (data) => { + this.winston.error('startStopStreaming() err ', { message: data }) + console.error(`GST stderr: ${data}`) + }) + + this.deviceStream.on('close', (code) => { + console.log(`GST process exited with code ${code}`) + this.winston.info('startStopStreaming() close ' + code) + this.deviceStream.stdin.pause() + this.deviceStream.kill() + this.resetVideo() + }) + + console.log('Started Video Streaming of ' + device) + this.winston.info('Started Video Streaming of ' + device) + } + // If enabled, start the camera heartbeat in either photo or video mode if (this.savedDevice.useCameraHeartbeat) { this.startInterval() } - console.log('Started Video Streaming of ' + device) - this.winston.info('Started Video Streaming of ' + device) - return callback(null, this.active, this.deviceAddresses) } else { // stop streaming @@ -281,9 +300,15 @@ class videoStream { if (this.savedDevice.useCameraHeartbeat) { clearInterval(this.intervalObj) } - this.deviceStream.stdin.pause() - this.deviceStream.kill() - this.resetVideo() + + if (this.savedDevice.usePhotoMode) { + console.log('Stopped photo mode') + this.resetVideo() + } else { + this.deviceStream.stdin.pause() + this.deviceStream.kill() + this.resetVideo() + } } return callback(null, this.active, this.deviceAddresses) } @@ -313,88 +338,327 @@ class videoStream { }, 1000) } + async startStopStreaming (active, device, height, width, format, rotation, bitrate, fps, useUDP, usePhotoMode, useUDPIP, useUDPPort, useTimestamp, useCameraHeartbeat, useMavControl, mavStreamSelected, mediaPath, callback) { + // if current state same, don't do anything + if (this.active === active) { + console.log('Video current same') + this.winston.info('Video current same') + return callback(null, this.active, this.deviceAddresses) + this.winston.info('From startstopstreaming, mediapath is: ', this.mediaPath) + } + // user wants to start or stop streaming + if (active) { + // check it's a valid video device + let found = false + if (this.devices !== null) { + for (let j = 0; j < this.devices.length; j++) { + if (device === this.devices[j].value) { + found = true + } + } + if (!found) { + console.log('No video device: ' + device) + this.winston.info('No video device: ' + device) + return callback(new Error('No video device: ' + device)) + } + } else { + console.log('No video devices in list') + this.winston.info('No video devices in list') + } + + this.active = true + this.savedDevice = { + device, + height, + width, + format, + bitrate, + fps, + rotation, + useUDP, + usePhotoMode, + useUDPIP, + useUDPPort, + useTimestamp, + useCameraHeartbeat, + useMavControl, + mavStreamSelected, + mediaPath + } + + // If photo mode was selected, start the libcamera server + if (this.savedDevice.usePhotoMode) { + // note that video device URL's are the alphanumeric characters only. So /dev/video0 -> devvideo0 + this.populateAddresses(device.replace(/\W/g, '')) + + const args = ['./python/photomode.py'] + + this.deviceStream = spawn('python3', args) + + try { + if (this.deviceStream === null) { + this.settings.setValue('videostream.active', false) + console.log('Error spawning photomode.py') + this.winston.info('Error spawning photomode.py') + return callback(null, this.active, this.deviceAddresses) + } + this.settings.setValue('videostream.active', this.active) + this.settings.setValue('videostream.savedDevice', this.savedDevice) + } catch (e) { + console.log(e) + this.winston.info(e) + } + + this.deviceStream.stdout.on('data', (data) => { + this.winston.info('startStopStreaming() data ' + data) + console.log(`GST stdout: ${data}`) + }) + + this.deviceStream.stderr.on('data', (data) => { + this.winston.error('startStopStreaming() err ', { message: data }) + console.error(`GST stderr: ${data}`) + }) + + this.deviceStream.on('close', (code) => { + console.log(`GST process exited with code ${code}`) + this.winston.info('startStopStreaming() close ' + code) + this.deviceStream.stdin.pause() + this.deviceStream.kill() + this.resetVideo() + }) + + console.log('Started Video Streaming of ' + device) + this.winston.info('Started Video Streaming of ' + device) + } else { + // note that video device URL's are the alphanumeric characters only. So /dev/video0 -> devvideo0 + this.populateAddresses(device.replace(/\W/g, '')) + + // rpi camera has different name under Ubuntu + if (await this.isUbuntu() && device === 'rpicam') { + device = '/dev/video0' + format = 'video/x-raw' + } + + const args = ['./python/rtsp-server.py', + '--video=' + device, + '--height=' + height, + '--width=' + width, + '--format=' + format, + '--bitrate=' + bitrate, + '--rotation=' + rotation, + '--fps=' + fps, + '--udp=' + ((useUDP === false) ? '0' : useUDPIP + ':' + useUDPPort.toString()) + ] + + if (useTimestamp) { + args.push('--timestamp') + } + + this.deviceStream = spawn('python3', args) + + try { + if (this.deviceStream === null) { + this.settings.setValue('videostream.active', false) + console.log('Error spawning rtsp-server.py') + this.winston.info('Error spawning rtsp-server.py') + return callback(null, this.active, this.deviceAddresses) + } + this.settings.setValue('videostream.active', this.active) + this.settings.setValue('videostream.savedDevice', this.savedDevice) + } catch (e) { + console.log(e) + this.winston.info(e) + } + + this.deviceStream.stdout.on('data', (data) => { + this.winston.info('startStopStreaming() data ' + data) + console.log(`GST stdout: ${data}`) + }) + + this.deviceStream.stderr.on('data', (data) => { + this.winston.error('startStopStreaming() err ', { message: data }) + console.error(`GST stderr: ${data}`) + }) + + this.deviceStream.on('close', (code) => { + console.log(`GST process exited with code ${code}`) + this.winston.info('startStopStreaming() close ' + code) + this.deviceStream.stdin.pause() + this.deviceStream.kill() + this.resetVideo() + }) + console.log('Started Video Streaming of ' + device) + this.winston.info('Started Video Streaming of ' + device) + } + + // If enabled, start the camera heartbeat in either photo or video mode + if (this.savedDevice.useCameraHeartbeat) { + this.startInterval() + } + + return callback(null, this.active, this.deviceAddresses) + } else { + // stop streaming + // if mavlink advertising is on, clear the interval + + if (this.savedDevice.useCameraHeartbeat) { + clearInterval(this.intervalObj) + } + + if (this.savedDevice.usePhotoMode) { + console.log('Stopped photo mode') + this.deviceStream.stdin.pause() + this.deviceStream.kill() + this.resetVideo() + } else { + this.deviceStream.stdin.pause() + this.deviceStream.kill() + this.resetVideo() + } + } + return callback(null, this.active, this.deviceAddresses) + } + + captureStillPhoto () { + // Capture a single still photo + console.log('Received capturestillphoto event') + this.deviceStream.kill('SIGUSR1') + + const senderSysId = this.targetSystem + const senderCompId = minimal.MavComponent.CAMERA + + // build a CAMERA_TRIGGER packet + const msg = new common.CameraTrigger() + + // Date.now() returns time in milliseconds + msg.timeUsec = BigInt(Date.now() * 1000) + msg.seq = this.photoSeq + + this.photoSeq++ + + this.eventEmitter.emit('cameratrigger', msg, senderSysId, senderCompId) + } + + sendCameraInformation (senderSysId, senderCompId, targetComponent) { + console.log('Responding to MAVLink request for CameraInformation') + this.winston.info('Responding to MAVLink request for CameraInformation') + + // const senderSysId = packet.header.sysid + // const senderCompId = minimal.MavComponent.CAMERA + // const targetComponent = packet.header.compid + + // build a CAMERA_INFORMATION packet + const msg = new common.CameraInformation() + + // TODO: implement missing attributes here + msg.timeBootMs = 0 + msg.vendorName = 0 + msg.modelName = 0 + msg.firmwareVersion = 0 + msg.focalLength = null + msg.sensorSizeH = null + msg.sensorSizeV = null + msg.resolutionH = this.savedDevice.width + msg.resolutionV = this.savedDevice.height + msg.lensId = 0 + // 256 = CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM (hard-coded for now until Rpanion gains more camera capabilities) + if (this.savedDevice.usePhotoMode) { + // 2 = CAMERA_CAP_FLAGS_CAPTURE_IMAGE + msg.flags = 2 + } else { + // 256 = CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM + msg.flags = 256 + } + + msg.camDefinitionVersion = 0 + msg.camDefinitionUri = '' + msg.gimbalDeviceId = 0 + this.eventEmitter.emit('camerainfo', msg, senderSysId, senderCompId, targetComponent) + } + + sendVideoStreamInformation (senderSysId, senderCompId, targetComponent) { + console.log('Responding to MAVLink request for VideoStreamInformation') + this.winston.info('Responding to MAVLink request for VideoStreamInformation') + + // const senderSysId = packet.header.sysid + // const senderCompId = minimal.MavComponent.CAMERA + // const targetComponent = packet.header.compid + + // build a VIDEO_STREAM_INFORMATION packet + const msg = new common.VideoStreamInformation() + // rpanion only supports a single stream, so streamId and count will always be 1 + msg.streamId = 1 + msg.count = 1 + + // msg.type and msg.uri need to be different depending on whether RTP or RTSP is selected + if (this.savedDevice.useUDP) { + // msg.type = 0 = VIDEO_STREAM_TYPE_RTSP + // msg.type = 1 = VIDEO_STREAM_TYPE_RTPUDP + msg.type = 1 + // For RTP, just send the destination UDP port instead of a full URI + msg.uri = this.savedDevice.useUDPPort.toString() + } else { + msg.type = 0 + msg.uri = `rtsp://${this.savedDevice.mavStreamSelected}:8554/${this.savedDevice.device}` + } + + // 1 = VIDEO_STREAM_STATUS_FLAGS_RUNNING + // 2 = VIDEO_STREAM_STATUS_FLAGS_THERMAL + msg.flags = 1 + msg.framerate = this.savedDevice.fps + msg.resolutionH = this.savedDevice.width + msg.resolutionV = this.savedDevice.height + msg.bitrate = this.savedDevice.bitrate + msg.rotation = this.savedDevice.rotation + // Rpanion doesn't collect field of view values, so just set to zero + msg.hfov = 0 + msg.name = this.savedDevice.device + + this.eventEmitter.emit('videostreaminfo', msg, senderSysId, senderCompId, targetComponent) + } + + sendCameraSettings (senderSysId, senderCompId, targetComponent) { + console.log('Responding to MAVLink request for CameraSettings') + this.winston.info('Responding to MAVLink request for CameraSettings') + // const senderSysId = packet.header.sysid + // const senderCompId = minimal.MavComponent.CAMERA + // const targetComponent = packet.header.compid + + // build a CAMERA_SETTINGS packet + const msg = new common.CameraSettings() + + msg.timeBootMs = 0 + // Camera modes: 0 = IMAGE, 1 = VIDEO, 2 = IMAGE_SURVEY + if (this.savedDevice.usePhotoMode) { + msg.modeId = 0 + } else { + msg.modeId = 1 + } + msg.zoomLevel = null + msg.focusLevel = null + + this.eventEmitter.emit('camerasettings', msg, senderSysId, senderCompId, targetComponent) + } + onMavPacket (packet, data) { // FC is active if (!this.active) { return } - if (data.targetComponent === minimal.MavComponent.CAMERA && - packet.header.msgid === common.CommandLong.MSG_ID && - data._param1 === common.CameraInformation.MSG_ID) { - console.log('Responding to MAVLink request for CameraInformation') - this.winston.info('Responding to MAVLink request for CameraInformation') - - const senderSysId = packet.header.sysid - const senderCompId = minimal.MavComponent.CAMERA - const targetComponent = packet.header.compid - - // build a CAMERA_INFORMATION packet - const msg = new common.CameraInformation() - - // TODO: implement missing attributes here - msg.timeBootMs = 0 - msg.vendorName = 0 - msg.modelName = 0 - msg.firmwareVersion = 0 - msg.focalLength = null - msg.sensorSizeH = null - msg.sensorSizeV = null - msg.resolutionH = this.savedDevice.width - msg.resolutionV = this.savedDevice.height - msg.lensId = 0 - // 256 = CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM (hard-coded for now until Rpanion gains more camera capabilities) - msg.flags = 256 - msg.camDefinitionVersion = 0 - msg.camDefinitionUri = '' - msg.gimbalDeviceId = 0 - this.eventEmitter.emit('camerainfo', msg, senderSysId, senderCompId, targetComponent) - - } else if (data.targetComponent === minimal.MavComponent.CAMERA && - packet.header.msgid === common.CommandLong.MSG_ID && - data._param1 === common.VideoStreamInformation.MSG_ID) { - - console.log('Responding to MAVLink request for VideoStreamInformation') - this.winston.info('Responding to MAVLink request for VideoStreamInformation') - - const senderSysId = packet.header.sysid - const senderCompId = minimal.MavComponent.CAMERA - const targetComponent = packet.header.compid - - // build a VIDEO_STREAM_INFORMATION packet - const msg = new common.VideoStreamInformation() - - // rpanion only supports a single stream, so streamId and count will always be 1 - msg.streamId = 1 - msg.count = 1 - - // msg.type and msg.uri need to be different depending on whether RTP or RTSP is selected - if (this.savedDevice.useUDP) { - // msg.type = 0 = VIDEO_STREAM_TYPE_RTSP - // msg.type = 1 = VIDEO_STREAM_TYPE_RTPUDP - msg.type = 1 - // For RTP, just send the destination UDP port instead of a full URI - msg.uri = this.savedDevice.useUDPPort.toString() - } else { - msg.type = 0 - msg.uri = `rtsp://${this.savedDevice.mavStreamSelected}:8554/${this.savedDevice.device}` + if (data.targetComponent === minimal.MavComponent.CAMERA && packet.header.msgid === common.CommandLong.MSG_ID) { + if (data._param1 === common.CameraInformation.MSG_ID) { + this.sendCameraInformation(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid) + } else if (data._param1 === common.VideoStreamInformation.MSG_ID && !this.savedDevice.usePhotoMode) { + this.sendVideoStreamInformation(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid) + } else if (data._param1 === common.CameraSettings.MSG_ID) { + this.sendCameraSettings(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid) + // 203 = MAV_CMD_DO_DIGICAM_CONTROL + } else if (data.command === 203) { + console.log('Received DoDigicamControl command') + this.captureStillPhoto(packet) } - - // 1 = VIDEO_STREAM_STATUS_FLAGS_RUNNING - // 2 = VIDEO_STREAM_STATUS_FLAGS_THERMAL - msg.flags = 1 - msg.framerate = this.savedDevice.fps - msg.resolutionH = this.savedDevice.width - msg.resolutionV = this.savedDevice.height - msg.bitrate = this.savedDevice.bitrate - msg.rotation = this.savedDevice.rotation - // Rpanion doesn't collect field of view values, so just set to zero - msg.hfov = 0 - msg.name = this.savedDevice.device - - this.eventEmitter.emit('videostreaminfo', msg, senderSysId, senderCompId, targetComponent) } } } - module.exports = videoStream diff --git a/server/videostream.test.js b/server/videostream.test.js index a5776838..c6ab6dbf 100644 --- a/server/videostream.test.js +++ b/server/videostream.test.js @@ -3,6 +3,12 @@ const settings = require('settings-store') const VideoStream = require('./videostream') const winston = require('./winstonconfig')(module) +const chai = require('chai') +const sinon = require('sinon') +const { expect } = chai +chai.use(require('sinon-chai')) +const { minimal, common } = require('node-mavlink') + describe('Video Functions', function () { it('#videomanagerinit()', function () { settings.clear() @@ -31,7 +37,9 @@ describe('Video Functions', function () { const vManager = new VideoStream(settings, winston) vManager.populateAddresses() - vManager.getVideoDevices(function (err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, selMavURI) { + vManager.getVideoDevices(function (err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, + SeluseUDP, SelusePhotoMode, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, + useCameraHeartbeat, useMavControl, selMavURI, selMediaPath) { assert.equal(err, null) assert.equal(active, false) assert.notEqual(seldevice, null) @@ -40,13 +48,17 @@ describe('Video Functions', function () { assert.notEqual(selbitrate, null) assert.notEqual(selfps, null) assert.equal(SeluseUDP, false) + assert.equal(SelusePhotoMode, false) assert.equal(SeluseUDPIP, '127.0.0.1') assert.equal(SeluseUDPPort, 5400) assert.equal(timestamp, false) assert.notEqual(fps, null) assert.notEqual(FPSMax, null) assert.notEqual(vidres, null) + assert.equal(useCameraHeartbeat, false) + assert.equal(useMavControl, false) assert.notEqual(selMavURI, null) + assert.equal(selMediaPath, '/home/pi/Rpanion-server/media/') done() }) }).timeout(5000) @@ -63,15 +75,352 @@ describe('Video Functions', function () { settings.clear() const vManager = new VideoStream(settings, winston) - vManager.startStopStreaming(true, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, true, false, '0', function (err, status, addresses) { + vManager.startStopStreaming(true, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, false, true, false, false, '0', '/home/pi/Rpanion-server/media/', function (err, status, addresses) { assert.equal(err, null) assert.equal(status, true) assert.notEqual(vManager.deviceStream.pid, null) - vManager.startStopStreaming(false, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, true, false, '0', function (err, status, addresses) { + vManager.startStopStreaming(false, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, false, true, false, false, '0', '/home/pi/Rpanion-server/media/', function (err, status, addresses) { assert.equal(err, null) assert.equal(status, false) done() }) }) }) + + describe('#videomanagerstartInterval()', () => { + let vManager + let setIntervalStub + let emitStub + + beforeEach(() => { + vManager = new VideoStream(settings, winston) + setIntervalStub = sinon.stub(global, 'setInterval') + emitStub = sinon.stub(vManager.eventEmitter, 'emit') + }) + + afterEach(() => { + sinon.restore() + }) + + it('should start an interval and emit a "cameraheartbeat" event', () => { + const intervalId = 12345 + setIntervalStub.returns(intervalId) + + vManager.startInterval() + + // Simulate the interval execution + const mavType = minimal.MavType.CAMERA + const autopilot = minimal.MavAutopilot.INVALID + const component = minimal.MavComponent.CAMERA + + // Call the interval function manually + const intervalFunction = setIntervalStub.firstCall.args[0] // Get the first argument (the callback) + intervalFunction() // Manually invoke the callback + + expect(vManager.intervalObj).to.equal(intervalId) + expect(emitStub).to.have.been.calledWith('cameraheartbeat', mavType, autopilot, component) + }) + }) + + describe('#videomanagercaptureStillPhoto()', () => { + let vManager + let emitStub + + beforeEach(() => { + vManager = new VideoStream(settings, winston) + // Mocking deviceStream with a kill method + vManager.deviceStream = { + kill: sinon.stub() + } + sinon.stub(Date, 'now').returns(1729137498022000) // Stub to return a fixed timestamp + emitStub = sinon.stub(vManager.eventEmitter, 'emit') + }) + + afterEach(() => { + sinon.restore() + }) + + it('should emit a "cameratrigger" event', () => { + // build a CAMERA_TRIGGER packet + const expectedMsg = new common.CameraTrigger() + expectedMsg.timeUsec = BigInt(Date.now() * 1000) + expectedMsg.seq = 0 + + vManager.captureStillPhoto() // Call the method under test + + expect(emitStub).to.have.been.calledWith('cameratrigger', expectedMsg) + }) + }) + + describe('#videomanagersendCameraInformation()', () => { + let vManager + let emitStub + + beforeEach(() => { + vManager = new VideoStream(settings, winston) + emitStub = sinon.stub(vManager.eventEmitter, 'emit') + + vManager.savedDevice = { + width: 1920, + height: 1080, + usePhotoMode: true + } + }) + + afterEach(() => { + sinon.restore() + }) + + it('should emit a "camerainfo" event', () => { + // build a CAMERA_INFORMATION packet + const expectedMsg = new common.CameraInformation() + + expectedMsg.timeBootMs = 0 + expectedMsg.vendorName = 0 + expectedMsg.modelName = 0 + expectedMsg.firmwareVersion = 0 + expectedMsg.focalLength = null + expectedMsg.sensorSizeH = null + expectedMsg.sensorSizeV = null + expectedMsg.resolutionH = vManager.savedDevice.width + expectedMsg.resolutionV = vManager.savedDevice.height + expectedMsg.lensId = 0 + // 256 = CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM (hard-coded for now until Rpanion gains more camera capabilities) + if (vManager.savedDevice.usePhotoMode) { + // 2 = CAMERA_CAP_FLAGS_CAPTURE_IMAGE + expectedMsg.flags = 2 + } else { + // 256 = CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM + expectedMsg.flags = 256 + } + + expectedMsg.camDefinitionVersion = 0 + expectedMsg.camDefinitionUri = '' + expectedMsg.gimbalDeviceId = 0 + + const senderSysId = 1 + const senderCompId = minimal.MavComponent.CAMERA + const targetComponent = 1 + + vManager.sendCameraInformation(senderSysId, senderCompId, targetComponent) // Call the method under test + expect(emitStub).to.have.been.calledWith('camerainfo', expectedMsg, senderSysId, senderCompId, targetComponent) + + // Test again with usePhotoMode = false + vManager.savedDevice.usePhotoMode = false + vManager.sendCameraInformation(senderSysId, senderCompId, targetComponent) // Call the method under test + expect(emitStub).to.have.been.calledWith('camerainfo', expectedMsg, senderSysId, senderCompId, targetComponent) + }) + }) + + describe('#videomanagersendVideoStreamInformation()', () => { + let vManager + let emitStub + + beforeEach(() => { + vManager = new VideoStream(settings, winston) + emitStub = sinon.stub(vManager.eventEmitter, 'emit') + + vManager.savedDevice = { + useUDP: true, + useUDPPort: 9000, + mavStreamSelected: 'localhost', + device: 'camera1', + fps: 30, + width: 1920, + height: 1080, + bitrate: 6000, + rotation: 0 + } + }) + + afterEach(() => { + sinon.restore() + }) + + it('should emit a "videostreaminfo" event', () => { + // build a VIDEO_STREAM_INFORMATION packet + const expectedMsg = new common.VideoStreamInformation() + + expectedMsg.streamId = 1 + expectedMsg.count = 1 + // expectedMsg.type and expectedMsg.uri need to be different depending on whether RTP or RTSP is selected + if (vManager.savedDevice.useUDP) { + // expectedMsg.type = 0 = VIDEO_STREAM_TYPE_RTSP + // expectedMsg.type = 1 = VIDEO_STREAM_TYPE_RTPUDP + expectedMsg.type = 1 + // For RTP, just send the destination UDP port instead of a full URI + expectedMsg.uri = vManager.savedDevice.useUDPPort.toString() + } else { + expectedMsg.type = 0 + expect.uri = `rtsp://${vManager.savedDevice.mavStreamSelected}:8554/${vManager.savedDevice.device}` + } + + // 1 = VIDEO_STREAM_STATUS_FLAGS_RUNNING + // 2 = VIDEO_STREAM_STATUS_FLAGS_THERMAL + expectedMsg.flags = 1 + expectedMsg.framerate = vManager.savedDevice.fps + expectedMsg.resolutionH = vManager.savedDevice.width + expectedMsg.resolutionV = vManager.savedDevice.height + expectedMsg.bitrate = vManager.savedDevice.bitrate + expectedMsg.rotation = vManager.savedDevice.rotation + // Rpanion doesn't collect field of view values, so just set to zero + expectedMsg.hfov = 0 + expectedMsg.name = vManager.savedDevice.device + + const senderSysId = 1 + const senderCompId = minimal.MavComponent.CAMERA + const targetComponent = 1 + + vManager.sendVideoStreamInformation(senderSysId, senderCompId, targetComponent) // Call the method under test + expect(emitStub).to.have.been.calledWith('videostreaminfo', expectedMsg, senderSysId, senderCompId, targetComponent) + + // Test again with useUDP = false + vManager.savedDevice.useUDP = false + vManager.sendVideoStreamInformation(senderSysId, senderCompId, targetComponent) // Call the method under test + expect(emitStub).to.have.been.calledWith('videostreaminfo', expectedMsg, senderSysId, senderCompId, targetComponent) + }) + }) + + describe('#videomanagersendCameraSettings()', () => { + let vManager + let emitStub + + beforeEach(() => { + vManager = new VideoStream(settings, winston) + emitStub = sinon.stub(vManager.eventEmitter, 'emit') + + vManager.savedDevice = { + usePhotoMode: true + } + }) + + afterEach(() => { + sinon.restore() + }) + + it('should emit a "camerasettings" event', () => { + // build a CAMERA_SETTINGS packet + const expectedMsg = new common.CameraSettings() + expectedMsg.timeBootMs = 0 + // Camera modes: 0 = IMAGE, 1 = VIDEO, 2 = IMAGE_SURVEY + if (vManager.savedDevice.usePhotoMode) { + expectedMsg.modeId = 0 + } else { + expectedMsg.modeId = 1 + } + expectedMsg.zoomLevel = null + expectedMsg.focusLevel = null + + const senderSysId = 1 + const senderCompId = minimal.MavComponent.CAMERA + const targetComponent = 1 + + vManager.sendCameraSettings(senderSysId, senderCompId, targetComponent) // Call the method under test + expect(emitStub).to.have.been.calledWith('camerasettings', expectedMsg, senderSysId, senderCompId, targetComponent) + + // Test again with usePhotoMode = false + vManager.savedDevice.usePhotoMode = false + vManager.sendCameraSettings(senderSysId, senderCompId, targetComponent) // Call the method under test + expect(emitStub).to.have.been.calledWith('camerasettings', expectedMsg, senderSysId, senderCompId, targetComponent) + }) + }) + + describe('#videomanageronMavPacket', function () { + let instance + let packet + let data + + beforeEach(function () { + instance = { + active: true, + savedDevice: { + usePhotoMode: false + }, + sendCameraInformation: sinon.spy(), + sendVideoStreamInformation: sinon.spy(), + sendCameraSettings: sinon.spy(), + captureStillPhoto: sinon.spy(), + + // Define the onMavPacket function within the instance + onMavPacket: function (packet, data) { + if (!this.active) { + return + } + + if (data.targetComponent === minimal.MavComponent.CAMERA && packet.header.msgid === common.CommandLong.MSG_ID) { + if (data._param1 === common.CameraInformation.MSG_ID) { + this.sendCameraInformation(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid) + } else if (data._param1 === common.VideoStreamInformation.MSG_ID && !this.savedDevice.usePhotoMode) { + this.sendVideoStreamInformation(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid) + } else if (data._param1 === common.CameraSettings.MSG_ID) { + this.sendCameraSettings(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid) + } else if (data.command === 203) { + console.log('Received DoDigicamControl command') + this.captureStillPhoto(packet) + } + } + } + } + + packet = { + header: { + sysid: 1, + compid: 1, + msgid: 76 // example msgid for CommandLong + } + } + + data = { + targetComponent: 100, + _param1: 0, + command: null + } + }) + + it('should not process if inactive', function () { + instance.active = false + instance.onMavPacket(packet, data) + expect(instance.sendCameraInformation.called).to.be.false + expect(instance.sendVideoStreamInformation.called).to.be.false + expect(instance.sendCameraSettings.called).to.be.false + expect(instance.captureStillPhoto.called).to.be.false + }) + + it('should send camera information when _param1 matches CameraInformation MSG_ID', function () { + data.targetComponent = minimal.MavComponent.CAMERA + data._param1 = common.CameraInformation.MSG_ID + instance.onMavPacket(packet, data) + expect(instance.sendCameraInformation.calledWith(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid)).to.be.true + }) + + it('should send video stream information when _param1 matches VideoStreamInformation MSG_ID and usePhotoMode is false', function () { + data.targetComponent = minimal.MavComponent.CAMERA + data._param1 = common.VideoStreamInformation.MSG_ID + instance.onMavPacket(packet, data) + expect(instance.sendVideoStreamInformation.calledWith(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid)).to.be.true + }) + + it('should send camera settings when _param1 matches CameraSettings MSG_ID', function () { + data.targetComponent = minimal.MavComponent.CAMERA + data._param1 = common.CameraSettings.MSG_ID + instance.onMavPacket(packet, data) + expect(instance.sendCameraSettings.calledWith(packet.header.sysid, minimal.MavComponent.CAMERA, packet.header.compid)).to.be.true + }) + + it('should capture still photo when command equals 203 (MAV_CMD_DO_DIGICAM_CONTROL)', function () { + data.targetComponent = minimal.MavComponent.CAMERA + data.command = 203 + instance.onMavPacket(packet, data) + expect(instance.captureStillPhoto.calledWith(packet)).to.be.true + }) + + it('should not process when targetComponent is not CAMERA', function () { + data.targetComponent = 200 // not the CAMERA component + instance.onMavPacket(packet, data) + expect(instance.sendCameraInformation.called).to.be.false + expect(instance.sendVideoStreamInformation.called).to.be.false + expect(instance.sendCameraSettings.called).to.be.false + expect(instance.captureStillPhoto.called).to.be.false + }) + }) }) diff --git a/src/video.js b/src/video.js index 8eb912e8..3664bf3b 100644 --- a/src/video.js +++ b/src/video.js @@ -13,6 +13,7 @@ class VideoPage extends basePage { super(props); this.state = { ifaces: [], + photoMode: false, dev: [], vidDeviceSelected: this.props.vidDeviceSelected, vidres: [], @@ -34,7 +35,8 @@ class VideoPage extends basePage { timestamp: false, enableCameraHeartbeat: false, mavStreamSelected: this.props.mavStreamSelected, - multicastString: " " + multicastString: " ", + mediaPath: "/home/pi/Rpanion-server/media/" } } @@ -42,6 +44,10 @@ class VideoPage extends basePage { fetch(`/api/videodevices`).then(response => response.json()).then(state => { this.setState(state); this.isMulticastUpdateIP(state.useUDPIP); this.loadDone() }); } + handleUsePhotoModeChange = (event) => { + this.setState({ photoMode: event.target.value==="photo" }); + } + handleVideoChange = (value, action) => { //new video device this.setState({ vidDeviceSelected: value, vidres: value.caps }); @@ -122,11 +128,30 @@ class VideoPage extends basePage { this.setState({ enableCameraHeartbeat: !this.state.enableCameraHeartbeat }); } + handleMavControlChange = (event) => { + // Allow MAVLink-connected devices to control the camera + this.setState({ enableMavControl: !this.state.enableMavControl }); + } + handleMavStreamChange = (value) => { //new value for selected stream IP this.setState({ mavStreamSelected: value }); } + handleMediaPathChange = (event) => { + this.setState({ mediaPath: event.target.value}); + } + + handleCaptureStill = (event) => { + fetch('/api/capturestillphoto', { + method: 'POST', + headers: { + 'Accept': 'application/json', + 'Content-Type': 'application/json', + } + }); + } + handleStreaming = (event) => { //user clicked start/stop streaming this.setState({ waiting: true }, () => { @@ -138,6 +163,7 @@ class VideoPage extends basePage { }, body: JSON.stringify({ active: !this.state.streamingStatus, + usePhotoMode: this.state.photoMode, device: this.state.vidDeviceSelected.value, height: this.state.vidResSelected.height, width: this.state.vidResSelected.width, @@ -151,7 +177,9 @@ class VideoPage extends basePage { useUDPPort: this.state.useUDPPort, useTimestamp: this.state.timestamp, useCameraHeartbeat: this.state.enableCameraHeartbeat, + useMavControl: this.state.enableMavControl, mavStreamSelected: this.state.mavStreamSelected.value, + mediaPath: this.state.mediaPath }) }).then(response => response.json()).then(state => { this.setState(state); this.setState({ waiting: false }) }); }); @@ -166,76 +194,107 @@ class VideoPage extends basePage {

Stream live video from any connected camera devices. Only 1 camera can be streamed at a time. Multicast IP addresses are supported in RTP mode.

Configuration

+
- +
- - + +
- - + +
-
- -
- -
- -
-
+
- + +
+
+ + +
+
+ + +
+
+
+ +
+
-
- +
- kbps + -
-
-
-
- -
- fps (max: {this.state.FPSMax}) +
+
+ +
+ kbps +
+
+
+ +
+ +
+
-
-
- -
- + +
+ fps (max: {this.state.FPSMax})
+
+
+ +
+ +
+
+
+ +
+ +
+
+
+
+ +
- -
- + +
+ +
+
+
+
+
+
-
+

MAVLink Video Streaming Service

Configuration for advertising the camera and associated video stream via MAVLink. See here for details.

@@ -243,8 +302,12 @@ class VideoPage extends basePage {
+ +
+ +
-
+
@@ -254,16 +317,25 @@ class VideoPage extends basePage {

-
-
- +
+
+
+ +
+
+
+
+
+
+
+ +
+
-
-
-

Connection strings for video stream

- +

Connection strings for video stream

+ + RTSP Streaming Addresses (for VLC, etc)