.gitignore

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

src/robot_interface/endpoints/video_sender.py

@@ -0,0 +1,8 @@
+import zmq
+
+from robot_interface.endpoints.socket_base import SocketBase
+
+class VideoSender(SocketBase):
+    def __init__(self, zmq_context, port=5556):
+        super(VideoSender, self).__init__("video")
+        self.create_socket(zmq_context, zmq.PUB, port)

src/robot_interface/main.py

@@ -1,9 +1,12 @@
 import logging
 import time
+import qi
+import threading
 
 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 +21,15 @@ def main_loop(context):
     main_receiver = MainReceiver(context)
     state.sockets.append(main_receiver)
 
+    video_sender = VideoSender(context)
+    state.sockets.append(video_sender)
+
+    # ip address of robot
+    robot_ip = "10.211.55.3"
+    # port of robot
+    port = 54321
+    start_video_rcv(robot_ip, port, video_sender)
+
     # Sockets that can run on the main thread. These sockets' endpoints should not block for long (say 50 ms at most).
     receivers = [main_receiver]
 
@@ -42,6 +54,55 @@ def main_loop(context):
             if time_spent_ms > 50:
                 logging.warn("Endpoint \"%s\" took too long (%.2f ms) on the main thread.", receiver.name, time_spent_ms)
 
+def start_video_rcv(robot_ip, port, socket):
+    """
+    Prepares arguments for retrieving video images from Pepper and starts video loop on a separate thread.
+
+    :param robot_ip: The ip address of the robot to connect with.
+    :type robot_ip: String
+    
+    :param port: The port of the robot.
+    :type port: int
+
+    :param socket: The ZMQ socket to send the video images over.
+    :type: Socket
+    """
+    socket.socket.setsockopt(zmq.CONFLATE,1)
+    
+    app = qi.Application(["cam", "--qi-url", "tcp://{}:{}".format(robot_ip, port)])
+    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=video_rcv_loop, args=(video, vid_stream_name, socket))
+    thread.start()
+
+def video_rcv_loop(vid_service, vid_stream_name, socket):
+    """
+    The main loop of retrieving video images from the robot.
+
+    :param vid_stream: The name of a camera subscription on the video service object vid_service
+    :type vid_stream: String
+
+    :param vid_service: The video service object that the active Qi session is connected to. 
+    :type vid_service: Object (Qi service object)
+
+    :param socket: The ZMQ socket to send the video images over.
+    :type: Socket
+    """
+    while not state.exit_event.is_set():
+        try:
+            img = vid_service.getImageRemote(vid_stream_name)
+            #Possibly limit images sent if queuing issues arise
+            socket.socket.send(img[6])
+        except:
+            logging.warn("Failed to retrieve video image from robot.")
 
 def main():
     context = zmq.Context()