From 144f41c0a40c981ee3aa123079f11faaad761326 Mon Sep 17 00:00:00 2001
From: phamthanhnam <phamthanhnam.ptn@gmail.com>
Date: Sun, 23 Jul 2023 15:15:36 +0700
Subject: [PATCH] Add support for Picamera2 (default on Raspberry Pi OS
 bullseye)

---
 docs/camera.rst    | 24 ++++++++++++++++
 docs/conf.py       |  1 +
 rtcbot/__init__.py |  2 +-
 rtcbot/camera.py   | 68 ++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 94 insertions(+), 1 deletion(-)

diff --git a/docs/camera.rst b/docs/camera.rst
index 04a3376..ca253de 100644
--- a/docs/camera.rst
+++ b/docs/camera.rst
@@ -87,6 +87,30 @@ above, and it returns exactly the same data for the frame::
 
 This means that if not using CVDisplay, you don't even need OpenCV installed to stream from you raspberry pi.
 
+PiCamera2
+++++++++++++++++
+
+This allows you to use the official raspberry pi camera, with libcamera stack (legacy camera interface disabled).
+This is default since Raspberry Pi OS bullseye, PiCamera2 also works with 64-bit OS.
+You can use the parameter hflip=1 to flip the camera horizontally, vflip=1 to flip vertically, or both to rotate 180 degrees.
+You can use it in exactly the same way as the OpenCV camera above, and it returns exactly the same data for the frame::
+
+    import asyncio
+    from rtcbot import PiCamera2, CVDisplay
+
+    camera = PiCamera2()
+    display = CVDisplay()
+
+    display.putSubscription(camera)
+
+    try:
+        asyncio.get_event_loop().run_forever()
+    finally:
+        camera.close()
+        display.close()
+
+This means that if not using CVDisplay, you don't even need OpenCV installed to stream from you raspberry pi.
+
 API
 ++++++++++++++++
 
diff --git a/docs/conf.py b/docs/conf.py
index e343972..3059d35 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -60,6 +60,7 @@
     "aiortc": ("https://aiortc.readthedocs.io/en/latest/", None),
     "inputs": ("https://inputs.readthedocs.io/en/latest/", None),
     "picamera": ("https://picamera.readthedocs.io/en/latest/", None),
+    "picamera2": ("https://datasheets.raspberrypi.com/camera/picamera2-manual.pdf", None),
 }
 
 # Add any paths that contain templates here, relative to this directory.
diff --git a/rtcbot/__init__.py b/rtcbot/__init__.py
index 1cf2f29..56c7ffc 100644
--- a/rtcbot/__init__.py
+++ b/rtcbot/__init__.py
@@ -1,7 +1,7 @@
 from .base import SubscriptionClosed
 from .connection import RTCConnection
 from .websocket import Websocket
-from .camera import CVCamera, PiCamera, CVDisplay
+from .camera import CVCamera, PiCamera, PiCamera2, CVDisplay
 from .audio import Microphone, Speaker
 from .inputs import Gamepad, Mouse, Keyboard
 from .arduino import SerialConnection
diff --git a/rtcbot/camera.py b/rtcbot/camera.py
index 1468fef..fd4e50a 100644
--- a/rtcbot/camera.py
+++ b/rtcbot/camera.py
@@ -134,12 +134,18 @@ class PiCamera(CVCamera):
 
     _log = logging.getLogger("rtcbot.PiCamera")
 
+    # Valid values for rotation are 0, 90, 180, 270
+    def __init__(self, rotation=0, **kwargs):
+        super().__init__(**kwargs)
+        self._rotation = rotation
+
     def _producer(self):
         import picamera
 
         with picamera.PiCamera() as cam:
             cam.resolution = (self._width, self._height)
             cam.framerate = self._fps
+            cam.rotation = self._rotation
             time.sleep(2)  # Why is this needed?
             self._log.debug("PiCamera Ready")
             self._setReady(True)
@@ -167,6 +173,68 @@ def _producer(self):
         self._log.info("Closed camera capture")
 
 
+class PiCamera2(CVCamera):
+    """
+    Instead of using OpenCV camera support, uses the picamera2 library for direct access to the Raspberry Pi's CSI camera.
+
+    The interface is identical to CVCamera. When testing code on a desktop computer, it can be useful to
+    have the code automatically choose the correct camera::
+
+        try:
+            import picamera2 # picamera2 import will fail if not on pi
+            cam = PiCamera2()
+        except ImportError:
+            cam = CVCamera()
+
+    This enables simple drop-in replacement between the two.
+
+    You can use the parameter hflip=True to flip the camera horizontally, vflip=True to flip vertically,
+    or both to rotate 180 degrees.
+    """
+
+    _log = logging.getLogger("rtcbot.PiCamera2")
+
+    def __init__(self, hflip=False, vflip=False, **kwargs):
+        super().__init__(**kwargs)
+        self._hflip = hflip
+        self._vflip = vflip
+
+    def _producer(self):
+        from picamera2 import Picamera2
+        from libcamera import Transform
+
+        with Picamera2() as cam:
+            cam.preview_configuration.transform = Transform(hflip=self._hflip, vflip=self._vflip)
+            cam.preview_configuration.main.size = (self._width, self._height)
+            cam.preview_configuration.display = None
+            cam.preview_configuration.main.format = 'RGB888'
+            if self._fps > 0:
+                cam.video_configuration.controls.FrameDurationLimits = (round(1000000/self._fps), round(1000000/self._fps))
+            cam.start()
+            time.sleep(2)
+            self._log.debug("PiCamera2 Ready")
+            self._setReady(True)
+
+            t = time.time()
+            i = 0
+            while not self._shouldClose:
+                frame = cam.capture_array()
+
+                # This optional function is given by the user. default is identity x->x
+                frame = self._processframe(frame)
+
+                # Set the frame arrival event
+                self._loop.call_soon_threadsafe(self._put_nowait, frame)
+
+                i += 1
+                if time.time() > t + 1:
+                    self._log.debug(" %d fps", i)
+                    i = 0
+                    t = time.time()
+        self._setReady(False)
+        self._log.info("Closed camera capture")
+
+
 class CVDisplay(BaseSubscriptionConsumer):
     """
     Displays the frames in an openCV `imshow` window