Browse Source
Merge pull request #182 from unitreerobotics/teleimager
Merge pull request #182 from unitreerobotics/teleimager
[v1.4] Teleimager, new televuer and moremain
silencht
4 months ago
committed by
GitHub
GitHub
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
18 changed files with 913 additions and 990 deletions
-
3.gitmodules
-
14CHANGELOG.md
-
20CHANGELOG_zh-CN.md
-
250README.md
-
210README_zh-CN.md
-
197teleop/image_server/image_client.py
-
321teleop/image_server/image_server.py
-
24teleop/robot_control/robot_arm.py
-
92teleop/robot_control/robot_arm_ik.py
-
2teleop/robot_control/robot_hand_brainco.py
-
189teleop/robot_control/robot_hand_inspire.py
-
53teleop/robot_control/robot_hand_unitree.py
-
1teleop/teleimager
-
376teleop/teleop_hand_and_arm.py
-
2teleop/televuer
-
10teleop/utils/episode_writer.py
-
45teleop/utils/ipc.py
-
54teleop/utils/motion_switcher.py
@ -1,197 +0,0 @@ |
|||||
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() |
|
||||
@ -1,321 +0,0 @@ |
|||||
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() |
|
||||
@ -1 +1 @@ |
|||||
Subproject commit 86367f8c9ba248f4065b959bfc53e5f36339bf6d |
|
||||
|
Subproject commit 948f65f6852410610483345e69715a0c673a99eb |
||||
@ -0,0 +1,54 @@ |
|||||
|
# for motion switcher |
||||
|
from unitree_sdk2py.core.channel import ChannelFactoryInitialize |
||||
|
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient |
||||
|
# for loco client |
||||
|
from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient |
||||
|
import time |
||||
|
|
||||
|
# MotionSwitcher used to switch mode between debug mode and ai mode |
||||
|
class MotionSwitcher: |
||||
|
def __init__(self): |
||||
|
ChannelFactoryInitialize(0) |
||||
|
self.msc = MotionSwitcherClient() |
||||
|
self.msc.SetTimeout(1.0) |
||||
|
self.msc.Init() |
||||
|
|
||||
|
def Enter_Debug_Mode(self): |
||||
|
try: |
||||
|
status, result = self.msc.CheckMode() |
||||
|
while result['name']: |
||||
|
self.msc.ReleaseMode() |
||||
|
status, result = self.msc.CheckMode() |
||||
|
time.sleep(1) |
||||
|
return status, result |
||||
|
except Exception as e: |
||||
|
return None, None |
||||
|
|
||||
|
def Exit_Debug_Mode(self): |
||||
|
try: |
||||
|
status, result = self.msc.SelectMode(nameOrAlias='ai') |
||||
|
return status, result |
||||
|
except Exception as e: |
||||
|
return None, None |
||||
|
|
||||
|
class LocoClientWrapper: |
||||
|
def __init__(self): |
||||
|
self.client = LocoClient() |
||||
|
self.client.SetTimeout(0.0001) |
||||
|
self.client.Init() |
||||
|
|
||||
|
def Enter_Damp_Mode(self): |
||||
|
self.client.Damp() |
||||
|
|
||||
|
def Move(self, vx, vy, vyaw): |
||||
|
self.client.Move(vx, vy, vyaw, continous_move=False) |
||||
|
|
||||
|
if __name__ == '__main__': |
||||
|
ChannelFactoryInitialize(0) |
||||
|
ms = MotionSwitcher() |
||||
|
status, result = ms.Enter_Debug_Mode() |
||||
|
print("Enter debug mode:", status, result) |
||||
|
time.sleep(5) |
||||
|
status, result = ms.Exit_Debug_Mode() |
||||
|
print("Exit debug mode:", status, result) |
||||
|
time.sleep(2) |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue