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_shape
self.fps = fps
self.serial_number = serial_number
self.enable_depth = enable_depth
align_to = rs.stream.color
self.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 None
depth_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 None
color_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 None
return color_image, depth_image
def 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_id
self.fps = fps
self.img_shape = img_shape
self.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 frames
if 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 success
def release(self):
self.cap.release()
def get_frame(self):
ret, color_image = self.cap.read()
if not ret:
return None
return color_image
class 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 = port
self.Unit_Test = Unit_Test
# Initialize head cameras
self.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 provided
self.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 socket
self.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 sent
self.time_window = 1.0 # Time window for FPS calculation (in seconds)
self.frame_times = deque() # Timestamps of frames sent within the time window
self.start_time = time.time() # Start time of the streaming
def _update_performance_metrics(self, current_time):
# Add current time to frame times deque
self.frame_times.append(current_time)
# Remove timestamps outside the time window
while self.frame_times and self.frame_times[0] < current_time - self.time_window:
self.frame_times.popleft()
# Increment frame count
self.frame_count += 1
def _print_performance_metrics(self, current_time):
if self.frame_count % 30 == 0:
elapsed_time = current_time - self.start_time
real_time_fps = len(self.frame_times) / self.time_window
logger_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.")
break
elif 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.")
break
head_frames.append(color_image)
if len(head_frames) != len(self.head_cameras):
break
head_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.")
break
elif 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.")
break
wrist_frames.append(color_image)
wrist_color = cv2.hconcat(wrist_frames)
# Concatenate head and wrist frames
full_color = cv2.hconcat([head_color, wrist_color])
else:
full_color = head_color
ret, buffer = cv2.imencode('.jpg', full_color)
if not ret:
logger_mp.error("[Image Server] Frame imencode is failed.")
continue
jpg_bytes = buffer.tobytes()
if self.Unit_Test:
timestamp = time.time()
frame_id = self.frame_count
header = struct.pack('dI', timestamp, frame_id) # 8-byte double, 4-byte unsigned int
message = header + jpg_bytes
else:
message = jpg_bytes
self.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 = True
self._image_show = image_show
self._server_address = server_address
self._port = port
self.tv_img_shape = tv_img_shape
self.wrist_img_shape = wrist_img_shape
self.tv_enable_shm = False
if 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 = True
self.wrist_enable_shm = False
if 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 parameters
self._enable_performance_eval = Unit_Test
if self._enable_performance_eval:
self._init_performance_metrics()
def _init_performance_metrics(self):
self._frame_count = 0 # Total frames received
self._last_frame_id = -1 # Last received frame ID
# Real-time FPS calculation using a time window
self._time_window = 1.0 # Time window size (in seconds)
self._frame_times = deque() # Timestamps of frames received within the time window
# Data transmission quality metrics
self._latencies = deque() # Latencies of frames within the time window
self._lost_frames = 0 # Total lost frames
self._total_frames = 0 # Expected total frames based on frame IDs
def _update_performance_metrics(self, timestamp, frame_id, receive_time):
# Update latency
latency = receive_time - timestamp
self._latencies.append(latency)
# Remove latencies outside the time window
while self._latencies and self._frame_times and self._latencies[0] < receive_time - self._time_window:
self._latencies.popleft()
# Update frame times
self._frame_times.append(receive_time)
# Remove timestamps outside the time window
while self._frame_times and self._frame_times[0] < receive_time - self._time_window:
self._frame_times.popleft()
# Update frame counts for lost frame calculation
expected_frame_id = self._last_frame_id + 1 if self._last_frame_id != -1 else frame_id
if frame_id != expected_frame_id:
lost = frame_id - expected_frame_id
if lost < 0:
logger_mp.info(f"[Image Client] Received out-of-order frame ID: {frame_id}")
else:
self._lost_frames += lost
logger_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_id
self._total_frames = frame_id + 1
self._frame_count += 1
def _print_performance_metrics(self, receive_time):
if self._frame_count % 30 == 0:
# Calculate real-time FPS
real_time_fps = len(self._frame_times) / self._time_window if self._time_window > 0 else 0
# Calculate latency metrics
if self._latencies:
avg_latency = sum(self._latencies) / len(self._latencies)
max_latency = max(self._latencies)
min_latency = min(self._latencies)
jitter = max_latency - min_latency
else:
avg_latency = max_latency = min_latency = jitter = 0
# Calculate lost frame rate
lost_frame_rate = (self._lost_frames / self._total_frames) * 100 if self._total_frames > 0 else 0
logger_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 socket
self._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 message
message = self._socket.recv()
receive_time = time.time()
if self._enable_performance_eval:
header_size = struct.calcsize('dI')
try:
# Attempt to extract header and image data
header = 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.")
continue
else:
# No header, entire message is image data
jpg_bytes = message
# Decode image
np_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.")
continue
if 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 = False
if 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 test
client = ImageClient(image_show = True, server_address='192.168.123.164', Unit_Test=False) # deployment test
client.receive_process()