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/video_sender.py b/src/robot_interface/endpoints/video_sender.py new file mode 100644 index 0000000..d0b7ca4 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/src/robot_interface/main.py b/src/robot_interface/main.py index a203357..3272ef4 100644 --- a/src/robot_interface/main.py +++ b/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()