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 27cb1e7..3419349 100644 --- a/src/robot_interface/endpoints/socket_base.py +++ b/src/robot_interface/endpoints/socket_base.py @@ -25,7 +25,7 @@ class SocketBase(object): self.socket = None # Set later by `create_socket` self.bound = None # Set later by `create_socket` - def create_socket(self, zmq_context, socket_type, port, bind=True): + def create_socket(self, zmq_context, socket_type, port, options=[], bind=True): """ Create a ZeroMQ socket. @@ -38,11 +38,19 @@ 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]] + :param bind: Whether to bind the socket or connect to it. :type bind: bool """ self.port = port self.socket = zmq_context.socket(socket_type) + + for option, arg in options: + self.socket.setsockopt(option,arg) + self.bound = bind if bind: self.socket.bind("tcp://*:{}".format(port)) 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 dacb1ce..8176740 100644 --- a/src/robot_interface/main.py +++ b/src/robot_interface/main.py @@ -6,6 +6,7 @@ import zmq from robot_interface.endpoints.actuation_receiver import ActuationReceiver from robot_interface.endpoints.main_receiver import MainReceiver +from robot_interface.endpoints.video_sender import VideoSender from robot_interface.state import state @@ -22,6 +23,11 @@ def main_loop(context): actuation_receiver = ActuationReceiver(context) state.sockets.append(actuation_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, actuation_receiver]