From 096fc1389f7b958c5e1165bfd2cdec5688ae8f44 Mon Sep 17 00:00:00 2001
From: Storm <stormluykx@gmail.com>
Date: Tue, 20 Jan 2026 12:44:16 +0100
Subject: [PATCH] chore: fixed sending Pepper imagery

---
 src/robot_interface/endpoints/video_sender.py | 67 ++++++++++++++++---
 1 file changed, 57 insertions(+), 10 deletions(-)

diff --git a/src/robot_interface/endpoints/video_sender.py b/src/robot_interface/endpoints/video_sender.py
index 3428882..d27e30b 100644
--- a/src/robot_interface/endpoints/video_sender.py
+++ b/src/robot_interface/endpoints/video_sender.py
@@ -1,6 +1,10 @@
+import struct
 import zmq
 import threading
 import logging
+import numpy as np
+
+import cv2
 
 from robot_interface.endpoints.socket_base import SocketBase
 from robot_interface.state import state
@@ -28,6 +32,9 @@ class VideoSender(SocketBase):
         """
         if not state.qi_session:
             logging.info("No Qi session available. Not starting video loop.")
+            logging.info("Starting test video stream from local webcam.")
+            thread = threading.Thread(target=self.test_video_stream)
+            thread.start()
             return
 
         video = state.qi_session.service("ALVideoDevice")
@@ -41,6 +48,35 @@ class VideoSender(SocketBase):
         thread = threading.Thread(target=self.video_rcv_loop, args=(video, vid_stream_name))
         thread.start()
 
+    def test_video_stream(self):
+        """
+        Test function to send video from a local webcam instead of the robot.
+        """
+        cap = cv2.VideoCapture(0)
+        if not cap.isOpened():
+            logging.error("Could not open webcam for video stream test.")
+            return
+
+        while not state.exit_event.is_set():
+
+            ret, frame = cap.read()
+            if not ret:
+                logging.warning("Failed to read frame from webcam.")
+                continue
+
+            if cv2.waitKey(1) & 0xFF == ord('q'): # << Add this: Updates the window
+                break
+            
+            small_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_AREA)
+            encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 70]
+            _, buffer = cv2.imencode('.jpg', small_frame, encode_param)
+
+            #cv2.imshow("Sender Preview", small_frame)  # << Add this: Opens the window
+
+            self.socket.send(buffer.tobytes())
+
+        cap.release()
+
     def video_rcv_loop(self, vid_service, vid_stream_name):
         """
         The main loop of retrieving video images from the robot.
@@ -51,14 +87,25 @@ class VideoSender(SocketBase):
         :param vid_stream_name: The name of a camera subscription on the video service object vid_service
         :type vid_stream_name: str
         """
-        while not state.exit_event.is_set():
-            state.active_event.wait()  # Wait until the system is not paused
-            if state.exit_event.is_set():
-                break
+        try:
+            while not state.exit_event.is_set():
+                try:
+                    img = vid_service.getImageRemote(vid_stream_name)
+                    if img is not None:
+                        raw_data = img[6]
+                        width = img[0]
+                        height = img[1]
 
-            try:
-                img = vid_service.getImageRemote(vid_stream_name)
-                #Possibly limit images sent if queuing issues arise
-                self.socket.send(img[settings.video_config.image_buffer])
-            except:
-                logging.warn("Failed to retrieve video image from robot.")
+                        frame = np.frombuffer(raw_data, dtype=np.uint8).reshape((height, width, 3))
+                        ret, jpeg_data = cv2.imencode('.jpg', frame)
+                        if ret:
+                            jpeg_bytes = jpeg_data.tobytes()
+                            
+                            self.socket.send(jpeg_bytes)
+                except:
+                    logging.warn("Failed to retrieve video image from robot.")
+        except KeyboardInterrupt:
+            logging.info("Video receiving loop interrupted by user.")
+        finally:
+            vid_service.unsubscribe(vid_stream_name)
+            logging.info("Unsubscribed from video stream.")
\ No newline at end of file