orisys-tech/orisys-sdk
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'dev' of yahoo.com.hk@192.168.192.190:D:\repo\orisyssdk_git into dev

Browse Source
This commit is contained in:
bluepulsar
2026-03-01 12:44:57 +08:00
parent ecf645dff0 b67ba3da5e
commit 1e3751c388
4 changed files with 560 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
five_sensors.py
+296
View File
@@ -0,0 +1,296 @@
"""
示例 4: 多传感器并行处理（多进程模式 - 不使用 MultiSensor）
本示例演示如何使用 multiprocessing 同时运行两个传感器：
- 每个传感器运行在独立的进程中
- 每个进程拥有独立的 CUDA 上下文
- 使用 multiprocessing.Process 和 Queue 进行进程间通信
架构说明：
进程 1 (传感器 0): 采集 → 光流 → HHD → 接触检测 → 队列
进程 2 (传感器 1): 采集 → 光流 → HHD → 接触检测 → 队列
↓
主进程: 显示 ← ───────────────┘
适用场景：多指触觉传感器、多视角采集、需要 GPU 加速的多路处理
注意事项：
- Windows 下必须在 if __name__ == '__main__' 中调用 mp.freeze_support()
- 每个传感器进程会占用独立的 GPU 显存
"""
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
import orisys
import numpy as np
import multiprocessing as mp
from multiprocessing import Queue, Process
import time
def sensor_worker(sensor_id, vid_src, config_name, cal_path, result_queue, stop_event, target_fps=None):
"""
传感器工作进程函数
Args:
sensor_id: 传感器编号
vid_src: 视频源（文件路径或摄像头索引）
config_name: 配置名称
cal_path: 标定路径
result_queue: 结果队列，用于向主进程发送数据
stop_event: 停止事件，用于通知工作进程退出
target_fps: 目标帧率；若设置则用定时器限速，使多传感器 FPS 接近一致
"""
try:
# 在每个进程中创建独立的传感器实例
sensor = orisys.Sensor(
vid_src,
isstitch=True,
cuda=True,
verbose=False, # 减少子进程输出
config_name=config_name,
cal_path=cal_path
)
print(f"传感器 {sensor_id} 已启动" + (f" (target_fps={target_fps})" if target_fps else ""))
frame_interval = 1.0 / target_fps if target_fps and target_fps > 0 else 0.0
while not stop_event.is_set():
t_start = time.perf_counter()
try:
# 步骤1: 获取图像
sensor.get_img()
# 步骤2: 计算形变场
sensor.compute_deformation()
# 步骤3: 计算触碰点
sensor.compute_contact()
# 步骤4: 读取信息
fps, flow, vnormal, img, contour, centroid, depth_map = sensor.read_info(
sensor.info.FPS,
sensor.info.VRAW,
sensor.info.VNORMAL,
sensor.info.IMG,
sensor.info.CONTOUR,
sensor.info.CENTROID,
sensor.info.DEPTH
)
# 步骤5: 准备数据字典（确保所有数据都是可序列化的）
data = {
'sensor_id': sensor_id,
'fps': fps,
'flow': flow.copy() if flow is not None else None,
'vnormal': vnormal.copy() if vnormal is not None else None,
'img': img.copy() if img is not None else None,
'contour': contour,
'centroid': centroid,
'depth_map': depth_map.copy() if depth_map is not None else None,
'timestamp': time.time()
}
# 步骤6: 发送数据到主进程（非阻塞）
try:
result_queue.put_nowait(data)
except Exception:
# 如果队列已满，跳过这一帧
pass
except Exception as e:
print(f"传感器 {sensor_id} 处理错误: {e}")
time.sleep(0.1)
# 步骤7: 精确限速（time.sleep 在 Windows 上精度仅 ~15.6ms，需留足余量后 spin-wait）
if frame_interval > 0:
deadline = t_start + frame_interval
remaining = deadline - time.perf_counter()
if remaining > 0.018:
time.sleep(remaining - 0.018) # coarse sleep; leave 18ms margin for Windows granularity
while time.perf_counter() < deadline:
pass # spin-wait for the last few ms
# 清理资源
sensor.disconnect()
print(f"传感器 {sensor_id} 已停止")
except Exception as e:
print(f"传感器 {sensor_id} 初始化失败: {e}")
result_queue.put({'sensor_id': sensor_id, 'error': str(e)})
def main():
# =========================================================================
# 1. 配置多个传感器
# =========================================================================
# 目标帧率：设为 None 不限速（可跑满 100+ FPS）；设为正数则 sleep 限速，使 FPS ≤ target_fps
target_fps = 30 # 不限速；设为 30 等可 cap 帧率
num_sensors = 5
sensor_configs = [
{
'sensor_id': 0,
'vid_src': './s1_480p.mp4',
'config_name': './config/prototype_of3_480p.json',
'cal_path': './config/dd01a_0_480p.npy'
},
{
'sensor_id': 1,
'vid_src': './s1_480p.mp4', # 可以改为不同的视频源或摄像头索引
'config_name': './config/prototype_of3_480p.json',
'cal_path': './config/dd01a_0_480p.npy'
},
{
'sensor_id': 2,
'vid_src': './s1_480p.mp4', # 可以改为不同的视频源或摄像头索引
'config_name': './config/prototype_of3_480p.json',
'cal_path': './config/dd01a_0_480p.npy'
},
{
'sensor_id': 3,
'vid_src': './s1_480p.mp4', # 可以改为不同的视频源或摄像头索引
'config_name': './config/prototype_of3_480p.json',
'cal_path': './config/dd01a_0_480p.npy'
},
{
'sensor_id': 4,
'vid_src': './s1_480p.mp4', # 可以改为不同的视频源或摄像头索引
'config_name': './config/prototype_of3_480p.json',
'cal_path': './config/dd01a_0_480p.npy'
},
]
# =========================================================================
# 2. 创建进程间通信对象
# =========================================================================
# 增大队列大小以缓冲更多帧，避免因主进程处理延迟导致的丢帧
result_queues = [Queue(maxsize=30) for _ in sensor_configs]
stop_events = [mp.Event() for _ in sensor_configs]
# =========================================================================
# 3. 启动传感器工作进程
# =========================================================================
processes = []
for i, config in enumerate(sensor_configs):
p = Process(
target=sensor_worker,
args=(i, config['vid_src'], config['config_name'], config['cal_path'],
result_queues[i], stop_events[i], target_fps)
)
p.start()
processes.append(p)
print(f"\n已启动 {len(processes)} 个传感器进程")
print("按 'q' 退出程序")
# 用于存储每个传感器的最新数据
latest_data = [None] * len(sensor_configs)
# 用于控制打印频率，减少I/O开销
print_counter = 0
print_interval = 30 # 每30帧打印一次，进一步减少I/O
# 用于控制按键检查频率
key_check_counter = 0
key_check_interval = 5 # 每5帧检查一次按键
try:
# =====================================================================
# 4. 主循环：从队列获取数据并显示
# =====================================================================
while True:
# 从所有队列中快速获取最新数据（高效清空队列，只保留最新帧）
for i, queue in enumerate(result_queues):
# 快速清空队列，只保留最后一帧
data = None
# 一次性获取所有可用数据，只保留最后一个
while True:
try:
data = queue.get_nowait()
except:
break
if data is not None:
latest_data[i] = data
# 显示所有传感器的数据
for data in latest_data:
if data is None or 'error' in data:
continue
sid = data['sensor_id']
# 显示形变场
if data['img'] is not None and data['flow'] is not None:
arrows = orisys.util.draw_arrows(
data['img'],
data['flow'],
threshold=5,
grid_spacing=10,
arrow_scale=1.0
)
cv2.imshow(f"Deformation {sid}", arrows)
# # 显示触碰点和区域
# if data['img'] is not None:
# image_with_foe = orisys.util.draw_contact(
# data['img'],
# data['contour'],
# data['centroid']
# )
# cv2.imshow(f"Contact Centroid {sid}", image_with_foe)
# # 显示深度图
# if data['depth_map'] is not None and data['depth_map'].size > 0:
# div_abs = data['depth_map']
# if div_abs.max() > div_abs.min():
# div_normalized = ((div_abs - div_abs.min()) /
# (div_abs.max() - div_abs.min()) * 255).astype(np.uint8)
# else:
# div_normalized = np.zeros_like(div_abs, dtype=np.uint8)
# cv2.imshow(f"Depth Map {sid}",
# cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
# 降低打印频率以减少I/O开销（在主循环级别控制）
print_counter += 1
if print_counter >= print_interval:
for data in latest_data:
if data is not None and 'error' not in data:
sid = data['sensor_id']
print(f"传感器 {sid}: FPS={data['fps']:5.1f}")
print_counter = 0
# 降低按键检查频率以减少开销
key_check_counter += 1
if key_check_counter >= key_check_interval:
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
print("\n退出程序...")
break
key_check_counter = 0
finally:
# =====================================================================
# 5. 清理资源
# =====================================================================
# 通知所有工作进程停止
for event in stop_events:
event.set()
# 等待所有进程结束
for p in processes:
p.join(timeout=2)
if p.is_alive():
p.terminate()
p.join()
cv2.destroyAllWindows()
print("所有传感器进程已停止")
if __name__ == '__main__':
# Windows 下多进程必需的初始化
mp.freeze_support()
main()
src/orisys/sensor.py
+143 -16
View File
@@ -7,9 +7,11 @@ and Helmholtz-Hodge decomposition.
"""
import os
import platform
import numpy as np
import cv2
import time
import threading
from enum import Enum, auto
from typing import Tuple
# from threading import Thread, Event
@@ -21,6 +23,10 @@ from .HHD import nHHD
from . import configs
from .get_depth import Contact3D
from .backend_availability import (check_cupy_available, check_opencl_available)
try:
import av # Optional: used for Linux camera capture path.
except ImportError:
av = None
class Sensor:
"""
@@ -128,6 +134,18 @@ class Sensor:
self.__contact3d = Contact3D(config_contact=self.__config_contact, verbose=self.verbose)
self.__hhd = None
self.poisson_backend = poisson_backend
self._use_pyav = False
self._av_container = None
self._av_stream = None
self._av_decoder = None
self.cap = None
self._is_live_camera = isinstance(self.vid_src, int)
self._capture_thread = None
self._capture_running = False
self._latest_frame = None
self._latest_frame_seq = 0
self._last_consumed_seq = -1
self._latest_frame_lock = threading.Lock()
# Setup video capture
self._open_video_source(override, box)
@@ -157,7 +175,7 @@ class Sensor:
# Simple direct read - buffer size of 1 should minimize frame accumulation
# Timing measurement helps identify if capture or processing is the bottleneck
t_capture_start = time.perf_counter()
ret, frame = self.cap.read()
ret, frame = self._read_frame()
t_capture_end = time.perf_counter()
self._last_capture_time = t_capture_end - t_capture_start
@@ -350,7 +368,20 @@ class Sensor:
def disconnect(self):
"""Release resources and stop async threads."""
if self.cap.isOpened():
self._capture_running = False
if self._capture_thread is not None:
self._capture_thread.join(timeout=1.0)
self._capture_thread = None
if self._use_pyav:
if self._av_container is not None:
self._av_container.close()
self._av_container = None
self._av_stream = None
self._av_decoder = None
if self.verbose:
print("Sensor disconnected.")
elif self.cap is not None and self.cap.isOpened():
self.cap.release()
if self.verbose:
print("Sensor disconnected.")
@@ -420,29 +451,57 @@ class Sensor:
def _open_video_source(self, override, box):
"""Initialize video capture and calibration."""
use_linux_camera_pyav = (
platform.system() == "Linux"
and isinstance(self.vid_src, int)
and av is not None
)
self.cap = cv2.VideoCapture(self.vid_src)
# Set buffer size to 1 to reduce latency and avoid frame accumulation
# Note: Not all backends support this, but it helps when available
try:
self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
except:
pass # Some backends don't support this property
self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
self.cap.set(cv2.CAP_PROP_FPS, self._config_fps)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0])
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1])
if use_linux_camera_pyav:
device = f"/dev/video{self.vid_src}"
try:
self._av_container = av.open(
device,
format="v4l2",
options={
"input_format": "mjpeg",
"video_size": f"{self._config_resolution_raw[0]}x{self._config_resolution_raw[1]}",
"framerate": str(self._config_fps),
},
)
self._av_stream = self._av_container.streams.video[0]
self._av_decoder = self._av_container.decode(self._av_stream)
self._use_pyav = True
if self.verbose:
print(f"Using PyAV Linux camera backend: {device}")
except Exception as exc:
if self.verbose:
print(f"PyAV camera open failed ({exc}), fallback to OpenCV backend.")
self._use_pyav = False
if not self._use_pyav:
self.cap = cv2.VideoCapture(self.vid_src)
# Set buffer size to 1 to reduce latency and avoid frame accumulation
# Note: Not all backends support this, but it helps when available
try:
self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
except Exception:
pass # Some backends don't support this property
self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
self.cap.set(cv2.CAP_PROP_FPS, self._config_fps)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0])
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1])
if self.verbose:
print("Connecting to video source...")
if not self.cap.isOpened():
if not self._is_source_open():
raise Exception("Failed to open video source.")
# Wait for first valid frame
frame = None
while frame is None:
ret, frame = self.cap.read()
ret, frame = self._read_frame_raw()
if not ret:
time.sleep(0.1)
@@ -463,6 +522,10 @@ class Sensor:
self.initial_img = cv2.flip(self.initial_img, 1)
self.initial_img = self.initial_img.astype(np.uint8)
# For live cameras, start a latest-frame capture thread to avoid latency buildup.
if self._is_live_camera:
self._start_capture_thread()
def _find_stitch_boxes(self, img_src, show=False):
"""Detect vision tile boxes with camera stabilization."""
@@ -470,7 +533,9 @@ class Sensor:
start_time = cv2.getTickCount()
while n_boxes < self._config_units:
ret, frame = self.cap.read()
ret, frame = self._read_frame_raw()
if not ret:
continue
v_mean = np.sum(cv2.mean(frame))
if abs(v_mean - v_mean0) < 5:
@@ -496,6 +561,68 @@ class Sensor:
self.stitcher.extract_tiles()
return True
def _is_source_open(self):
if self._use_pyav:
return self._av_container is not None
return self.cap is not None and self.cap.isOpened()
def _read_frame(self):
if self._capture_running:
# Startup race guard: wait briefly until capture thread publishes first frame.
if self._latest_frame is None:
deadline = time.perf_counter() + 1.0
while time.perf_counter() < deadline:
with self._latest_frame_lock:
if self._latest_frame is not None:
break
time.sleep(0.001)
# Return latest frame immediately (do not wait for next camera tick).
# This keeps end-to-end latency minimal; caller may receive repeated
# frames if processing runs faster than camera FPS.
with self._latest_frame_lock:
frame = self._latest_frame
seq = self._latest_frame_seq
if frame is not None:
self._last_consumed_seq = seq
return True, frame.copy()
return False, None
return self._read_frame_raw()
def _read_frame_raw(self):
if self._use_pyav:
if self._av_decoder is None:
return False, None
try:
frame = next(self._av_decoder)
except StopIteration:
return False, None
except Exception:
return False, None
return True, frame.to_ndarray(format="bgr24")
if self.cap is None:
return False, None
return self.cap.read()
def _start_capture_thread(self):
if self._capture_thread is not None:
return
self._capture_running = True
self._capture_thread = threading.Thread(target=self._capture_loop, daemon=True)
self._capture_thread.start()
if self.verbose:
print("Latest-frame capture thread started.")
def _capture_loop(self):
while self._capture_running:
ret, frame = self._read_frame_raw()
if not ret:
time.sleep(0.001)
continue
with self._latest_frame_lock:
self._latest_frame = frame
self._latest_frame_seq += 1
# =========================================================================
# Private - Utilities
test_cam.py
+75
View File
@@ -0,0 +1,75 @@
import cv2
import time
# Toggle these to test whether exposure is limiting delivered FPS.
USE_MANUAL_EXPOSURE = False
MANUAL_EXPOSURE = -6 # Typical V4L2/OpenCV manual range is negative values.
cap = cv2.VideoCapture(4, cv2.CAP_V4L2)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
cap.set(cv2.CAP_PROP_FPS, 30)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
# For V4L2 in OpenCV: auto-exposure is usually 3, manual is usually 1.
if USE_MANUAL_EXPOSURE:
ok_auto = cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 1)
ok_exp = cap.set(cv2.CAP_PROP_EXPOSURE, MANUAL_EXPOSURE)
else:
ok_auto = cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 3)
ok_exp = True
actual_fourcc = int(cap.get(cv2.CAP_PROP_FOURCC))
print(f"Backend: {cap.getBackendName()}")
print(
f"FOURCC: {chr(actual_fourcc & 0xFF)}"
f"{chr((actual_fourcc >> 8) & 0xFF)}"
f"{chr((actual_fourcc >> 16) & 0xFF)}"
f"{chr((actual_fourcc >> 24) & 0xFF)}"
)
print(f"Resolution: {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))}x{int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")
print(f"FPS: {cap.get(cv2.CAP_PROP_FPS)}")
print(
f"Exposure mode: {'manual' if USE_MANUAL_EXPOSURE else 'auto'} "
f"(set_auto_ok={ok_auto}, set_exp_ok={ok_exp})"
)
print(f"AutoExposure(readback): {cap.get(cv2.CAP_PROP_AUTO_EXPOSURE)}")
print(f"Exposure(readback): {cap.get(cv2.CAP_PROP_EXPOSURE)}")
frame_count = 0
start = time.perf_counter()
while cap.isOpened():
t0 = time.perf_counter()
ok = cap.grab()
t1 = time.perf_counter()
if not ok:
break
ret, frame = cap.retrieve()
t2 = time.perf_counter()
if not ret:
continue
cv2.imshow("webcam", frame)
t3 = time.perf_counter()
key = cv2.waitKey(1) & 0xFF
t4 = time.perf_counter()
frame_count += 1
if frame_count % 30 == 0:
elapsed = t4 - start
print(
f"Frame {frame_count}: "
f"grab={1000*(t1-t0):.1f}ms, "
f"retrieve={1000*(t2-t1):.1f}ms, "
f"imshow={1000*(t3-t2):.1f}ms, "
f"waitKey={1000*(t4-t3):.1f}ms, "
f"avg_fps={frame_count/elapsed:.1f}"
)
if key == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
test_ffmpeg.py
+46
View File
@@ -0,0 +1,46 @@
import av
import cv2
import time
DEVICE = "/dev/video4"
TARGET_FPS = 30
WIDTH = 1280
HEIGHT = 720
# Open v4l2 device with same style options as ffmpeg CLI
container = av.open(
DEVICE,
format="v4l2",
options={
"input_format": "mjpeg",
"video_size": f"{WIDTH}x{HEIGHT}",
"framerate": str(TARGET_FPS),
},
)
stream = container.streams.video[0]
frame_count = 0
t_start = time.perf_counter()
for frame in container.decode(stream):
t0 = time.perf_counter()
img = frame.to_ndarray(format="bgr24") # OpenCV-compatible
t1 = time.perf_counter()
cv2.imshow("pyav_cam", img)
key = cv2.waitKey(1) & 0xFF
t2 = time.perf_counter()
frame_count += 1
if frame_count % 30 == 0:
elapsed = t2 - t_start
print(
f"Frame {frame_count}: convert={1000*(t1-t0):.1f}ms, "
f"imshow+waitKey={1000*(t2-t1):.1f}ms, avg_fps={frame_count/elapsed:.1f}"
)
if key == ord("q"):
break
container.close()
cv2.destroyAllWindows()