【机器人+相机通讯】宇树科技相机通信

https://github.com/unitreerobotics/xr_teleoperate/blob/main/README_zh-CN.md

相机驱动与服务端

https://github.com/unitreerobotics/xr_teleoperate/blob/main/teleop/image_server/image_server.py

其中相机如果是realsense, 安装好驱动后，可以使用命令查看序列号

rs-enumerate-devices

 服务端代码

import cv2
import zmq
import time
import struct
from collections import deque
import numpy as np
import pyrealsense2 as rs
import logging_mp
logger_mp = logging_mp.get_logger(__name__, level=logging_mp.DEBUG)class RealSenseCamera(object):def __init__(self, img_shape, fps, serial_number=None, enable_depth=False) -> None:"""img_shape: [height, width]serial_number: serial number"""self.img_shape = img_shapeself.fps = fpsself.serial_number = serial_numberself.enable_depth = enable_depthalign_to = rs.stream.colorself.align = rs.align(align_to)self.init_realsense()def init_realsense(self):self.pipeline = rs.pipeline()config = rs.config()if self.serial_number is not None:config.enable_device(self.serial_number)config.enable_stream(rs.stream.color, self.img_shape[1], self.img_shape[0], rs.format.bgr8, self.fps)if self.enable_depth:config.enable_stream(rs.stream.depth, self.img_shape[1], self.img_shape[0], rs.format.z16, self.fps)profile = self.pipeline.start(config)self._device = profile.get_device()if self._device is None:logger_mp.error('[Image Server] pipe_profile.get_device() is None .')if self.enable_depth:assert self._device is not Nonedepth_sensor = self._device.first_depth_sensor()self.g_depth_scale = depth_sensor.get_depth_scale()self.intrinsics = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()def get_frame(self):frames = self.pipeline.wait_for_frames()aligned_frames = self.align.process(frames)color_frame = aligned_frames.get_color_frame()if self.enable_depth:depth_frame = aligned_frames.get_depth_frame()if not color_frame:return Nonecolor_image = np.asanyarray(color_frame.get_data())# color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)depth_image = np.asanyarray(depth_frame.get_data()) if self.enable_depth else Nonereturn color_image, depth_imagedef release(self):self.pipeline.stop()class OpenCVCamera():def __init__(self, device_id, img_shape, fps):"""decive_id: /dev/video* or *img_shape: [height, width]"""self.id = device_idself.fps = fpsself.img_shape = img_shapeself.cap = cv2.VideoCapture(self.id, cv2.CAP_V4L2)self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.img_shape[0])self.cap.set(cv2.CAP_PROP_FRAME_WIDTH,  self.img_shape[1])self.cap.set(cv2.CAP_PROP_FPS, self.fps)# Test if the camera can read framesif not self._can_read_frame():logger_mp.error(f"[Image Server] Camera {self.id} Error: Failed to initialize the camera or read frames. Exiting...")self.release()def _can_read_frame(self):success, _ = self.cap.read()return successdef release(self):self.cap.release()def get_frame(self):ret, color_image = self.cap.read()if not ret:return Nonereturn color_imageclass ImageServer:def __init__(self, config, port = 5555, Unit_Test = False):"""config example1:{'fps':30                                                          # frame per second'head_camera_type': 'opencv',                                     # opencv or realsense'head_camera_image_shape': [480, 1280],                           # Head camera resolution  [height, width]'head_camera_id_numbers': [0],                                    # '/dev/video0' (opencv)'wrist_camera_type': 'realsense', 'wrist_camera_image_shape': [480, 640],                           # Wrist camera resolution  [height, width]'wrist_camera_id_numbers': ["218622271789", "241222076627"],      # realsense camera's serial number}config example2:{'fps':30                                                          # frame per second'head_camera_type': 'realsense',                                  # opencv or realsense'head_camera_image_shape': [480, 640],                            # Head camera resolution  [height, width]'head_camera_id_numbers': ["218622271739"],                       # realsense camera's serial number'wrist_camera_type': 'opencv', 'wrist_camera_image_shape': [480, 640],                           # Wrist camera resolution  [height, width]'wrist_camera_id_numbers': [0,1],                                 # '/dev/video0' and '/dev/video1' (opencv)}If you are not using the wrist camera, you can comment out its configuration, like this below:config:{'fps':30                                                          # frame per second'head_camera_type': 'opencv',                                     # opencv or realsense'head_camera_image_shape': [480, 1280],                           # Head camera resolution  [height, width]'head_camera_id_numbers': [0],                                    # '/dev/video0' (opencv)#'wrist_camera_type': 'realsense', #'wrist_camera_image_shape': [480, 640],                           # Wrist camera resolution  [height, width]#'wrist_camera_id_numbers': ["218622271789", "241222076627"],      # serial number (realsense)}"""logger_mp.info(config)self.fps = config.get('fps', 30)self.head_camera_type = config.get('head_camera_type', 'opencv')self.head_image_shape = config.get('head_camera_image_shape', [480, 640])      # (height, width)self.head_camera_id_numbers = config.get('head_camera_id_numbers', [0])self.wrist_camera_type = config.get('wrist_camera_type', None)self.wrist_image_shape = config.get('wrist_camera_image_shape', [480, 640])    # (height, width)self.wrist_camera_id_numbers = config.get('wrist_camera_id_numbers', None)self.port = portself.Unit_Test = Unit_Test# Initialize head camerasself.head_cameras = []if self.head_camera_type == 'opencv':for device_id in self.head_camera_id_numbers:camera = OpenCVCamera(device_id=device_id, img_shape=self.head_image_shape, fps=self.fps)self.head_cameras.append(camera)elif self.head_camera_type == 'realsense':for serial_number in self.head_camera_id_numbers:camera = RealSenseCamera(img_shape=self.head_image_shape, fps=self.fps, serial_number=serial_number)self.head_cameras.append(camera)else:logger_mp.warning(f"[Image Server] Unsupported head_camera_type: {self.head_camera_type}")# Initialize wrist cameras if providedself.wrist_cameras = []if self.wrist_camera_type and self.wrist_camera_id_numbers:if self.wrist_camera_type == 'opencv':for device_id in self.wrist_camera_id_numbers:camera = OpenCVCamera(device_id=device_id, img_shape=self.wrist_image_shape, fps=self.fps)self.wrist_cameras.append(camera)elif self.wrist_camera_type == 'realsense':for serial_number in self.wrist_camera_id_numbers:camera = RealSenseCamera(img_shape=self.wrist_image_shape, fps=self.fps, serial_number=serial_number)self.wrist_cameras.append(camera)else:logger_mp.warning(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}")# Set ZeroMQ context and socketself.context = zmq.Context()self.socket = self.context.socket(zmq.PUB)self.socket.bind(f"tcp://*:{self.port}")if self.Unit_Test:self._init_performance_metrics()for cam in self.head_cameras:if isinstance(cam, OpenCVCamera):logger_mp.info(f"[Image Server] Head camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}")elif isinstance(cam, RealSenseCamera):logger_mp.info(f"[Image Server] Head camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}")else:logger_mp.warning("[Image Server] Unknown camera type in head_cameras.")for cam in self.wrist_cameras:if isinstance(cam, OpenCVCamera):logger_mp.info(f"[Image Server] Wrist camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}")elif isinstance(cam, RealSenseCamera):logger_mp.info(f"[Image Server] Wrist camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}")else:logger_mp.warning("[Image Server] Unknown camera type in wrist_cameras.")logger_mp.info("[Image Server] Image server has started, waiting for client connections...")def _init_performance_metrics(self):self.frame_count = 0  # Total frames sentself.time_window = 1.0  # Time window for FPS calculation (in seconds)self.frame_times = deque()  # Timestamps of frames sent within the time windowself.start_time = time.time()  # Start time of the streamingdef _update_performance_metrics(self, current_time):# Add current time to frame times dequeself.frame_times.append(current_time)# Remove timestamps outside the time windowwhile self.frame_times and self.frame_times[0] < current_time - self.time_window:self.frame_times.popleft()# Increment frame countself.frame_count += 1def _print_performance_metrics(self, current_time):if self.frame_count % 30 == 0:elapsed_time = current_time - self.start_timereal_time_fps = len(self.frame_times) / self.time_windowlogger_mp.info(f"[Image Server] Real-time FPS: {real_time_fps:.2f}, Total frames sent: {self.frame_count}, Elapsed time: {elapsed_time:.2f} sec")def _close(self):for cam in self.head_cameras:cam.release()for cam in self.wrist_cameras:cam.release()self.socket.close()self.context.term()logger_mp.info("[Image Server] The server has been closed.")def send_process(self):try:while True:head_frames = []for cam in self.head_cameras:if self.head_camera_type == 'opencv':color_image = cam.get_frame()if color_image is None:logger_mp.error("[Image Server] Head camera frame read is error.")breakelif self.head_camera_type == 'realsense':color_image, depth_iamge = cam.get_frame()if color_image is None:logger_mp.error("[Image Server] Head camera frame read is error.")breakhead_frames.append(color_image)if len(head_frames) != len(self.head_cameras):breakhead_color = cv2.hconcat(head_frames)if self.wrist_cameras:wrist_frames = []for cam in self.wrist_cameras:if self.wrist_camera_type == 'opencv':color_image = cam.get_frame()if color_image is None:logger_mp.error("[Image Server] Wrist camera frame read is error.")breakelif self.wrist_camera_type == 'realsense':color_image, depth_iamge = cam.get_frame()if color_image is None:logger_mp.error("[Image Server] Wrist camera frame read is error.")breakwrist_frames.append(color_image)wrist_color = cv2.hconcat(wrist_frames)# Concatenate head and wrist framesfull_color = cv2.hconcat([head_color, wrist_color])else:full_color = head_colorret, buffer = cv2.imencode('.jpg', full_color)if not ret:logger_mp.error("[Image Server] Frame imencode is failed.")continuejpg_bytes = buffer.tobytes()if self.Unit_Test:timestamp = time.time()frame_id = self.frame_countheader = struct.pack('dI', timestamp, frame_id)  # 8-byte double, 4-byte unsigned intmessage = header + jpg_byteselse:message = jpg_bytesself.socket.send(message)if self.Unit_Test:current_time = time.time()self._update_performance_metrics(current_time)self._print_performance_metrics(current_time)except KeyboardInterrupt:logger_mp.warning("[Image Server] Interrupted by user.")finally:self._close()if __name__ == "__main__":config = {'fps': 30,'head_camera_type': 'opencv','head_camera_image_shape': [480, 1280],  # Head camera resolution'head_camera_id_numbers': [0],'wrist_camera_type': 'opencv','wrist_camera_image_shape': [480, 640],  # Wrist camera resolution'wrist_camera_id_numbers': [2, 4],}server = ImageServer(config, Unit_Test=False)server.send_process()

同一网段的客户端

import cv2
import zmq
import numpy as np
import time
import struct
from collections import deque
from multiprocessing import shared_memory
import logging_mp
logger_mp = logging_mp.get_logger(__name__)class ImageClient:def __init__(self, tv_img_shape = None, tv_img_shm_name = None, wrist_img_shape = None, wrist_img_shm_name = None, image_show = False, server_address = "192.168.123.164", port = 5555, Unit_Test = False):"""tv_img_shape: User's expected head camera resolution shape (H, W, C). It should match the output of the image service terminal.tv_img_shm_name: Shared memory is used to easily transfer images across processes to the Vuer.wrist_img_shape: User's expected wrist camera resolution shape (H, W, C). It should maintain the same shape as tv_img_shape.wrist_img_shm_name: Shared memory is used to easily transfer images.image_show: Whether to display received images in real time.server_address: The ip address to execute the image server script.port: The port number to bind to. It should be the same as the image server.Unit_Test: When both server and client are True, it can be used to test the image transfer latency, \network jitter, frame loss rate and other information."""self.running = Trueself._image_show = image_showself._server_address = server_addressself._port = portself.tv_img_shape = tv_img_shapeself.wrist_img_shape = wrist_img_shapeself.tv_enable_shm = Falseif self.tv_img_shape is not None and tv_img_shm_name is not None:self.tv_image_shm = shared_memory.SharedMemory(name=tv_img_shm_name)self.tv_img_array = np.ndarray(tv_img_shape, dtype = np.uint8, buffer = self.tv_image_shm.buf)self.tv_enable_shm = Trueself.wrist_enable_shm = Falseif self.wrist_img_shape is not None and wrist_img_shm_name is not None:self.wrist_image_shm = shared_memory.SharedMemory(name=wrist_img_shm_name)self.wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = self.wrist_image_shm.buf)self.wrist_enable_shm = True# Performance evaluation parametersself._enable_performance_eval = Unit_Testif self._enable_performance_eval:self._init_performance_metrics()def _init_performance_metrics(self):self._frame_count = 0  # Total frames receivedself._last_frame_id = -1  # Last received frame ID# Real-time FPS calculation using a time windowself._time_window = 1.0  # Time window size (in seconds)self._frame_times = deque()  # Timestamps of frames received within the time window# Data transmission quality metricsself._latencies = deque()  # Latencies of frames within the time windowself._lost_frames = 0  # Total lost framesself._total_frames = 0  # Expected total frames based on frame IDsdef _update_performance_metrics(self, timestamp, frame_id, receive_time):# Update latencylatency = receive_time - timestampself._latencies.append(latency)# Remove latencies outside the time windowwhile self._latencies and self._frame_times and self._latencies[0] < receive_time - self._time_window:self._latencies.popleft()# Update frame timesself._frame_times.append(receive_time)# Remove timestamps outside the time windowwhile self._frame_times and self._frame_times[0] < receive_time - self._time_window:self._frame_times.popleft()# Update frame counts for lost frame calculationexpected_frame_id = self._last_frame_id + 1 if self._last_frame_id != -1 else frame_idif frame_id != expected_frame_id:lost = frame_id - expected_frame_idif lost < 0:logger_mp.info(f"[Image Client] Received out-of-order frame ID: {frame_id}")else:self._lost_frames += lostlogger_mp.warning(f"[Image Client] Detected lost frames: {lost}, Expected frame ID: {expected_frame_id}, Received frame ID: {frame_id}")self._last_frame_id = frame_idself._total_frames = frame_id + 1self._frame_count += 1def _print_performance_metrics(self, receive_time):if self._frame_count % 30 == 0:# Calculate real-time FPSreal_time_fps = len(self._frame_times) / self._time_window if self._time_window > 0 else 0# Calculate latency metricsif self._latencies:avg_latency = sum(self._latencies) / len(self._latencies)max_latency = max(self._latencies)min_latency = min(self._latencies)jitter = max_latency - min_latencyelse:avg_latency = max_latency = min_latency = jitter = 0# Calculate lost frame ratelost_frame_rate = (self._lost_frames / self._total_frames) * 100 if self._total_frames > 0 else 0logger_mp.info(f"[Image Client] Real-time FPS: {real_time_fps:.2f}, Avg Latency: {avg_latency*1000:.2f} ms, Max Latency: {max_latency*1000:.2f} ms, \Min Latency: {min_latency*1000:.2f} ms, Jitter: {jitter*1000:.2f} ms, Lost Frame Rate: {lost_frame_rate:.2f}%")def _close(self):self._socket.close()self._context.term()if self._image_show:cv2.destroyAllWindows()logger_mp.info("Image client has been closed.")def receive_process(self):# Set up ZeroMQ context and socketself._context = zmq.Context()self._socket = self._context.socket(zmq.SUB)self._socket.connect(f"tcp://{self._server_address}:{self._port}")self._socket.setsockopt_string(zmq.SUBSCRIBE, "")logger_mp.info("Image client has started, waiting to receive data...")try:while self.running:# Receive messagemessage = self._socket.recv()receive_time = time.time()if self._enable_performance_eval:header_size = struct.calcsize('dI')try:# Attempt to extract header and image dataheader = message[:header_size]jpg_bytes = message[header_size:]timestamp, frame_id = struct.unpack('dI', header)except struct.error as e:logger_mp.warning(f"[Image Client] Error unpacking header: {e}, discarding message.")continueelse:# No header, entire message is image datajpg_bytes = message# Decode imagenp_img = np.frombuffer(jpg_bytes, dtype=np.uint8)current_image = cv2.imdecode(np_img, cv2.IMREAD_COLOR)if current_image is None:logger_mp.warning("[Image Client] Failed to decode image.")continueif self.tv_enable_shm:np.copyto(self.tv_img_array, np.array(current_image[:, :self.tv_img_shape[1]]))if self.wrist_enable_shm:np.copyto(self.wrist_img_array, np.array(current_image[:, -self.wrist_img_shape[1]:]))if self._image_show:height, width = current_image.shape[:2]resized_image = cv2.resize(current_image, (width // 2, height // 2))cv2.imshow('Image Client Stream', resized_image)if cv2.waitKey(1) & 0xFF == ord('q'):self.running = Falseif self._enable_performance_eval:self._update_performance_metrics(timestamp, frame_id, receive_time)self._print_performance_metrics(receive_time)except KeyboardInterrupt:logger_mp.info("Image client interrupted by user.")except Exception as e:logger_mp.warning(f"[Image Client] An error occurred while receiving data: {e}")finally:self._close()if __name__ == "__main__":# example1# tv_img_shape = (480, 1280, 3)# img_shm = shared_memory.SharedMemory(create=True, size=np.prod(tv_img_shape) * np.uint8().itemsize)# img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=img_shm.buf)# img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = img_shm.name)# img_client.receive_process()# example2# Initialize the client with performance evaluation enabled# client = ImageClient(image_show = True, server_address='127.0.0.1', Unit_Test=True) # local testclient = ImageClient(image_show = True, server_address='192.168.123.164', Unit_Test=False) # deployment testclient.receive_process()