Merge branch 'feat/ri-receive-video' into 'dev'

Implemented receiving video in RI from robot

See merge request ics/sp/2025/n25b/pepperplus-ri!4

.gitignore

@@ -214,3 +214,6 @@ __marimo__/
 
 # Streamlit
 .streamlit/secrets.toml
+
+.DS_Store
+

src/robot_interface/endpoints/socket_base.py

@@ -15,7 +15,7 @@ class SocketBase(object):
         self.name = name
         self.socket = None
 
-    def create_socket(self, zmq_context, socket_type, port):
+    def create_socket(self, zmq_context, socket_type, port, options=[]):
         """
         Create a ZeroMQ socket.
 
@@ -27,8 +27,16 @@ class SocketBase(object):
 
         :param port: The port to use.
         :type port: int
+
+        :param options: A list of options to be set on the socket. The list contains tuples where the first element contains the option
+        and the second the value, for example (zmq.CONFLATE, 1).
+        :type options: list[tuple[int, int]]
         """
         self.socket = zmq_context.socket(socket_type)
+
+        for option, arg in options:
+            self.socket.setsockopt(option,arg)
+
         self.socket.connect("tcp://localhost:{}".format(port))
 
     def close(self):

src/robot_interface/endpoints/video_sender.py

@@ -0,0 +1,49 @@
+import zmq
+import threading
+import qi
+import logging
+
+from robot_interface.endpoints.socket_base import SocketBase
+from robot_interface.state import state
+
+
+class VideoSender(SocketBase):
+    def __init__(self, zmq_context, port=5556):
+        super(VideoSender, self).__init__("video")
+        self.create_socket(zmq_context, zmq.PUB, port, [(zmq.CONFLATE,1)])
+    
+    def start_video_rcv(self):
+        """
+        Prepares arguments for retrieving video images from Pepper and starts video loop on a separate thread.
+        """        
+        app = qi.Application()
+        app.start()
+        session = app.session
+
+        video = session.service("ALVideoDevice")
+
+        camera_index = 0
+        kQVGA = 2
+        kRGB = 11
+        FPS = 15
+        vid_stream_name = video.subscribeCamera("Pepper Video", camera_index, kQVGA, kRGB, FPS)
+        thread = threading.Thread(target=self.video_rcv_loop, args=(video, vid_stream_name))
+        thread.start()
+
+    def video_rcv_loop(self, vid_service, vid_stream_name):
+        """
+        The main loop of retrieving video images from the robot.
+
+        :param vid_service: The video service object that the active Qi session is connected to. 
+        :type vid_service: Object (Qi service object)
+
+        :param vid_stream_name: The name of a camera subscription on the video service object vid_service
+        :type vid_stream_name: String
+        """
+        while not state.exit_event.is_set():
+            try:
+                img = vid_service.getImageRemote(vid_stream_name)
+                #Possibly limit images sent if queuing issues arise
+                self.socket.send(img[6])
+            except:
+                logging.warn("Failed to retrieve video image from robot.")

src/robot_interface/main.py

@@ -4,6 +4,7 @@ import time
 import zmq
 
 from robot_interface.endpoints.main_receiver import MainReceiver
+from robot_interface.endpoints.video_sender import VideoSender
 from robot_interface.state import state
 
 
@@ -18,6 +19,11 @@ def main_loop(context):
     main_receiver = MainReceiver(context)
     state.sockets.append(main_receiver)
 
+    video_sender = VideoSender(context)
+    state.sockets.append(video_sender)
+
+    video_sender.start_video_rcv()
+
     # Sockets that can run on the main thread. These sockets' endpoints should not block for long (say 50 ms at most).
     receivers = [main_receiver]