DIY Video Stream am Raspberry Car (Teil 7)

tl;dr: Das Auto sendet einzelne Kamerabilder als Bytes an ein Canvas im Controller.

...oder wie ich eine mobile Kamera baute (Teil 7)

Eine handliche Steuerung gibt es seit dem letzten Blog–Artikel schon, und jetzt geht es los mit dem Fahren per Kamera ohne Sichtkontakt. Drei wichtige Ziele erreichen wir in diesem Blog–Artikel:

Das Kamerabild soll auf dem Controller sichtbar sein.
Das Auto soll sich allein per Kamerabild steuern lassen.
Die Steuersoftware auf dem Auto soll automatisch gestartet werden, sobald der Raspberry Pi eingeschaltet wird.

Es bleiben natürlich wie immer Wünsche offen. Der Code ist ein Kompromiss aus Performance, Lesbarkeit, Komplexität und meiner Zeit für das Projekt. Fühle dich frei, damit dein Projekt zu starten und ihn zu verbessern.

Übrigens gibt es bereits einen anderen Raspberry Pi IP Camera Blog–Artikel. Der wichtigste Unterschied ist, dass dort der Raspberry Pi der Web–Server ist. Dass heißt man muss mit dem Browser direkt auf den Raspberry zugreifen (können). Firewalls und Subnetze sind da ein Hindernis. Hier sind das Auto und der Controller beide Clients und treffen sich auf einem separaten Server, wie in diesem Blog–Artikel erklärt wird.

Damit der Code in dem Artikel leichter zu lesen ist und sich besser kopieren lässt, besteht er aus wenigen großen Dateien. Meinem Drang, Dateien herauszutrennen anstatt Code zu kopieren, konnte ich gerade widerstehen, damit du beim Lesen nicht ständig hin und her springen musst. Normalerweise empfehle ich eine andere Dateistruktur.

Car.py Service

Auto einschalten, einloggen per SSH, car.py starten, endlich losfahren… das nervt. Daher soll die Steuersoftware als Service (aka Daemon) im Hintergrund laufen und automatisch beim Einschalten gestartet werden. Wie in dieser Dokumentation beschrieben, erstellt man einen Service mit einer speziellen Datei in /etc/systemd/system/. Danach muss man den Dienst noch aktivieren mit systemctl enable car.

# place in /etc/systemd/system/
# and systemctl enable car
# check with systemctl status car
# control with systemctl start/stop car
[Unit]
Description=Raspberry Pi Car Client
After=network.target
 
[Service]
# check path with `which python3`
# replace the IP by domain if available
ExecStart=/usr/bin/python3 -u car.py http://192.168.0.103:3000
WorkingDirectory=/home/pi/socket.io/
StandardOutput=inherit
StandardError=inherit
Restart=always
RestartSec=5
User=pi
 
[Install]
WantedBy=multi-user.target

Generalprobe am Emulator

Mit einem lokalen Emulator entwickelt es sich viel schneller als auf dem Raspberry. Außerdem ist der Code weniger komplex und daher ein gute Einstieg. Ein paar Design–Entscheidungen vorweg, die im Code ein Rolle spielen:

Die Bildübertragung wird gestoppt, wenn das Auto nicht gesteuert wird.
Mit einem Blick zum Auto sieht man, ob die Kamera benutzt wird (per LED).
Als Controller nehme ich ein Handy mit kleinem Display und eher schmalbandiger Datenleitung an.

In der aktuellen Implementierung wird jeder Frame als einzelnes JPEG als Socket.IO Event übertragen. Einen Videocodec verwende ich nicht, obwohl damit die Bildqualität bei gleicher Datenrate höher sein müsste.

Die Steuersoftware (bzw. der Emulator) hat nun zwei Threads, die parallel laufen: einer empfängt und verarbeitet die Befehle vom Controller, der andere sendet ein Bild von der Kamera nach dem anderen. Die Synchronisation zwischen den Threads ist minimal: kam ein Stop–Befehl, soll nach 10 Sekunden kein Bild mehr übertragen werden.

An meinem Laptop und an dem Raspberry Pi besitzt die Kamera eine Status LED, die die Nutzung der Kamera anzeigt. Wenn man den Zugriff auf die Kamera stoppt, erlischt die LED. Damit haben wir eine fertige Status LED, indem wir die Kamera effizient nutzen – perfekt.

Hier kommt der Emulator–Code. Für die eigentliche Steuersoftware können wir fast alle Teile wiederverwenden.

import cv2
import socketio
import sys
import threading
import time
 
class AtomicInteger:
    """
        thread-safe integer
        thanks to https://stackoverflow.com/questions/23547604/python-counter-atomic-increment
    """
    def __init__(self, value=0):
        self._value = value
        self._lock = threading.Lock()
 
    @property
    def value(self):
        with self._lock:
            return self._value
 
    @value.setter
    def value(self, v):
        with self._lock:
            self._value = v
            return self._value
 
class Camera:
    """
        camera to be enabled/disabled and auto-calibrated
    """
    def __init__(self):
        self._camera = None
 
    def _enableUnlessEnabled(self):
        if self._camera == None:
            self._camera = cv2.VideoCapture(0)
 
    def disable(self):
        self._camera = None
 
    def capture(self):
        # power up camera
        self._enableUnlessEnabled()
        # Grab a single frame of video
        ret, frame = self._camera.read()
        # Resize frame of video to 1/4 size
        frameSmall = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)
        # encode data as JPEG
        encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50]
        frameJpeg = cv2.imencode('.jpg', frameSmall, encode_param)
        # get bytes
        frameBytes = frameJpeg[1].tobytes()
        return frameBytes
 
class VideoStreamer:
    """
        Meant to be run by background worker
        to send camera captures to clients.
    """
    def __init__(self, sio):
        self._sio = sio
        self._camera = Camera()
        self._stopAtTime = AtomicInteger(0)
 
    @property
    def stopAtTime(self):
        """
            point in time after which no frames are sent
        """
        return self._stopAtTime.value
 
    @stopAtTime.setter
    def stopAtTime(self, seconds):
        self._stopAtTime.value = seconds
 
    def stream(self):
        # limit frame rate to upper bound
        maxFps = 5
        minSendingTime = 1/maxFps
        while True:
            now = time.time()
            if self.stopAtTime > round(now):
                self._sio.emit('video', self._camera.capture())
                sendingTime = time.time() - now
                delay = minSendingTime - sendingTime
                if delay > 0:
                    # wait before sending next frame
                    # to stay at maximal frame rate
                    time.sleep(delay)
            else:
                self._camera.disable()
                # busy-waiting: works for now, is not efficient
                time.sleep(1)
 
sio = socketio.Client()
videoStreamer = VideoStreamer(sio)
 
@sio.event
def connect():
    print('connected to server')
 
@sio.event
def command(cmd):
    isStopped = cmd == 'stop'
    # stop video stream after 10 s (unless car is moving)
    videoStreamer.stopAtTime = round(time.time()) + (10 if isStopped else 99999999) 
    print('command: ', cmd)
 
@sio.event
def disconnect():
    print('disconnected from server')
 
 
if __name__ == '__main__':
    try:
        sio.connect(sys.argv[1] if len(sys.argv) > 1 else 'http://localhost:3000')
        videoStreamWorker = threading.Thread(target=videoStreamer.stream, args=(), daemon=True)
        videoStreamWorker.start()
        sio.wait()
    except KeyboardInterrupt:
        print('bye')

Video–Stream darstellen

Unsere Bildfolge kommt als Bytes am Controller an und muss angezeigt werden… wenn der Server sie auch sendet. Alle Bilder werden (noch) über den Server umgeleitet; als Binärdaten.

const express = require('express');
const app = express();
const http = require('http').createServer(app);
const io = require('socket.io')(http);
 
app.get('/', (req, res) => {
  res.sendFile(__dirname + '/static/controller.html');
});
 
app.use('/', express.static('static'));
 
io.on('connection', socket => {
    console.log('something connected');
    // send command to car to indicate that controller is ready (starts video stream)
    socket.broadcast.binary(false).volatile.emit('command', 'stop')
    // broadcast commands
    socket.on('command', cmd => socket.broadcast.binary(false).volatile.emit('command', cmd));
    socket.on('video', frame => socket.broadcast.binary(true).volatile.emit('video', frame));    // stop on disconnect
    socket.on('disconnect', () => socket.broadcast.binary(false).volatile.emit('command', 'stop'));
});
 
http.listen(3000, () => {
  console.log('listening on *:3000');
});

Die JPEG–Bytes werden unverändert übertragen und insbesondere nicht in Base64 kodiert. Wenn die Bytes empfangen werden, wird direkt ein JPEG daraus erzeugt. Das JPEG wird anschließend skaliert, in ein Canvas gezeichnet und wieder gelöscht. Damit lässt sich ein flüssiges Video darstellen.

<html>
  <head>
    <title>Socket.IO Joystick</title>
  </head>
  <body style="background-color: #00AEEF;">
    <center>
      <canvas style="z-index: 999999;" id="video" width="800" height="600"></canvas>    </center>
    <div id="joystick"/>
    <script src="https://cdn.jsdelivr.net/npm/socket.io-client@2/dist/socket.io.js"></script>
    <script src="./nipplejs.js"></script>
    <script>
        var joystick = nipplejs.create({
            zone: document.getElementById('joystick'),
            mode: 'static',
            position: {left: '50%', top: '75%'},
            color: 'white',
            size: Math.round(0.5 * Math.min(window.innerWidth, window.innerHeight))
        });
        var socket = io();
        function command(cmd) {
            socket.binary(false).emit('command', cmd);
        }
        var frame = new Image();        var frameUrl;        var canvas = document.getElementById('video');        canvas.height = Math.round(0.5 * window.innerHeight);        canvas.width = window.innerWidth - 20;        var video = canvas.getContext('2d');        frame.onload = function () {          var widthRation = canvas.width / frame.width;          var heigthRation = canvas.height / frame.height;          var ration = Math.min(widthRation, heigthRation);          video.drawImage(frame, 0, 0, ration * frame.width, ration * frame.height);          (URL || webkitURL).revokeObjectURL(url); // release memory        };        // thanks to https://stackoverflow.com/questions/30120250/draw-jpeg-array-directly-onto-a-canvas        socket.on('video', function(bytes) {          var blob = new Blob([bytes], {type: "image/jpeg"});          url = (URL || webkitURL).createObjectURL(blob);          frame.src = url;        });        joystick.on('end', function() { command('stop'); });
        joystick.on('dir:up', function() { command('foreward'); });
        joystick.on('dir:left', function() { command('left'); });
        joystick.on('dir:right', function() { command('right'); });
        joystick.on('dir:down', function() { command('backward'); });
    </script>
  </body>
</html>

Die Steuersoftware car.py

Die folgende car.py lässt sich auf dem Raspberry Auto ausführen. Die meisten Teile sind die aus Teil 5 (abgesehen von winzigen Änderungen) oder aus dem Emulator in diesem bereits bekannt. Nur die Klasse Camera ist neu.

import cv2
import socketio
import RPi.GPIO as GPIO
from picamera import PiCamerafrom picamera.array import PiRGBArrayimport time
import sys
import threading
import io
 
print("""
Socket.io client for the car.
Connects to server and receives commands and events.
""")
 
class AtomicInteger:
    """
        thread-safe interger
        thanks to https://stackoverflow.com/questions/23547604/python-counter-atomic-increment
    """
    def __init__(self, value=0):
        self._value = value
        self._lock = threading.Lock()
 
    @property
    def value(self):
        with self._lock:
            return self._value
 
    @value.setter
    def value(self, v):
        with self._lock:
            self._value = v
            return self._value
 
class Camera:    """        camera to be enabled/disabled and auto-calibrated    """    def __init__(self):        self._camera = None        self._width = 320        self._height = 256        # camera is attached to car upside down        self._rotationMatrix = cv2.getRotationMatrix2D(            (self._width/2, self._height/2), 180, 1.0)     def _enableUnlessEnabled(self):        if self._camera == None:            self._camera = PiCamera()            self._camera.resolution = (self._width, self._height)            self._camera.framerate = 60            # https://picamera.readthedocs.io/en/release-1.13/recipes1.html#capturing-consistent-images            # high ISO for lower shutter speed thus better images while moving            self._camera.iso = 1200            # Wait for the automatic gain control to settle            time.sleep(2)            # Now fix the values            self._camera.shutter_speed = self._camera.exposure_speed            self._camera.exposure_mode = 'off'            awb_gains = self._camera.awb_gains            self._camera.awb_mode = 'off'            self._camera.awb_gains = awb_gains     def disable(self):        if self._camera != None:            self._camera.close()            self._camera = None     def capture(self):        self._enableUnlessEnabled()        # capture frame        capture = PiRGBArray(self._camera, size=(self._width, self._height))        self._camera.capture(capture, format="bgr")        frame = capture.array        # rotate (since camera is attached upside down)        frameRotated = cv2.warpAffine(frame, self._rotationMatrix, (self._width, self._height))        # compress        encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50]        frameJpeg = cv2.imencode('.jpg', frameRotated, encode_param)        # get bytes        frameBytes = frameJpeg[1].tobytes()        return frameBytes 
class VideoStreamer:
    """
        Meant to be run by background worker
        to send camera captures to clients.
    """
    def __init__(self, sio):
        self._sio = sio
        self._camera = Camera()
        self._stopAtTime = AtomicInteger(0)
 
    @property
    def stopAtTime(self):
        return self._stopAtTime.value
 
    @stopAtTime.setter
    def stopAtTime(self, seconds):
        self._stopAtTime.value = seconds
 
    def stream(self):
        maxFps = 5
        minSendingTime = 1/maxFps
        while True:
            now = time.time()
            if self.stopAtTime > round(time.time()):
                self._sio.emit('video', self._camera.capture())
                sendingTime = time.time() - now
                delay = minSendingTime - sendingTime
                if delay > 0:
                    time.sleep(delay)
            else:
                self._camera.disable()
                time.sleep(1)
 
class Wheel:
    """
        wheel(s) on one side of the car
    """
 
    def __init__(self, enPin: int, inForewardPin: int, inBackwardPin: int):
        GPIO.setup(inForewardPin, GPIO.OUT)
        self.inForewardPin = inForewardPin
        GPIO.output(inForewardPin, False)
 
        self.inBackwardPin = inBackwardPin
        GPIO.setup(inBackwardPin, GPIO.OUT)
        GPIO.output(inBackwardPin, False)
 
        GPIO.setup(enPin, GPIO.OUT)
        self.pwm = GPIO.PWM(enPin, 100)
        self.pwm.start(0)
        self.pwm.ChangeDutyCycle(0)
 
    def setSpeed(self, speed: int):
        """
            speed might be -3, -2, -1, 0, 1, 2, 3
        """
        force = min(3, abs(speed))
        isForewards = speed > 0
        if (isForewards):
            GPIO.output(self.inBackwardPin, False)
            GPIO.output(self.inForewardPin, True)
        else:
            GPIO.output(self.inForewardPin, False)
            GPIO.output(self.inBackwardPin, force > 0)
        if force > 0:
            self.pwm.ChangeDutyCycle(70 + (10 * force))
        else:
            self.pwm.ChangeDutyCycle(0)
 
if __name__ == '__main__':
    GPIO.setmode(GPIO.BOARD)
    RightWheels = Wheel(3, 5, 7)
    LeftWheels = Wheel(8, 10, 12)
 
    def drive(speedLeft, speedRight):
        LeftWheels.setSpeed(speedLeft)
        RightWheels.setSpeed(speedRight)
 
    sio = socketio.Client()
    videoStreamer = VideoStreamer(sio)
    try:
        @sio.event
        def connect():
            # a LED would be handy
            drive(0, 0)
 
        @sio.event
        def command(data):
            isStopped = data == 'stop'
            # stop video stream after 10 s (unless car is moving)
            videoStreamer.stopAtTime = round(time.time()) + (10 if isStopped else 99999999) 
            if data == 'left':
                drive(-1, 1)
            elif data == 'right':
                drive(1, -1)
            elif data == 'foreward':
                drive(3, 3)
            elif data == 'backward':
                drive(-3, -3)
            elif data == 'stop':
                drive(0, 0)
            else:
                print("unknown command '%s'" % data)
                drive(0, 0)
 
        @sio.event
        def disconnect():
            # a LED would be handy
            drive(0, 0)
 
        sio.connect(sys.argv[1] if len(sys.argv) > 1 else 'http://localhost:3000')
        videoStreamWorker = threading.Thread(target=videoStreamer.stream, args=(), daemon=True)
        videoStreamWorker.start()
        sio.wait()
    except KeyboardInterrupt:
        LeftWheels.setSpeed(0)
        RightWheels.setSpeed(0)
        GPIO.cleanup()
        print('bye')

Das Auto lässt sich jetzt ohne Sichtverbindung nur über die Kamera steuern. Ziel erreicht – oder sagen wir fast. Auch bei starker Kompression bekomme ich von dem Auto nur bescheidene 2,2 fps. Eine Fahrt ist damit eine Herausforderung. Verantwortlich ist fast ausschließlich der Abruf des Frames von der Kamera mit camera.capture. Entweder muss ich die Kamera effizienter ansteuern, nicht das billigste Kamera–Modul verwenden, oder es ist nicht möglich. Das will ich noch herausfinden.

Gerne kannst du mir schreiben bei Fragen oder Tipps. Danke für's Lesen und viel Spaß beim Basteln.