diff --git a/.gitignore b/.gitignore index c3002d8..9a65a1b 100644 --- a/.gitignore +++ b/.gitignore @@ -214,3 +214,6 @@ __marimo__/ # Streamlit .streamlit/secrets.toml + +.DS_Store + diff --git a/src/robot_interface/endpoints/socket_base.py b/src/robot_interface/endpoints/socket_base.py index a5124f6..f86b3ec 100644 --- a/src/robot_interface/endpoints/socket_base.py +++ b/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): diff --git a/src/robot_interface/endpoints/video_sender.py b/src/robot_interface/endpoints/video_sender.py new file mode 100644 index 0000000..793385b --- /dev/null +++ b/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.") diff --git a/src/robot_interface/main.py b/src/robot_interface/main.py index a203357..fb48040 100644 --- a/src/robot_interface/main.py +++ b/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]