add multisensor
This commit is contained in:
unknown
+79
-81
@@ -9,19 +9,16 @@ independent CUDA contexts.
|
||||
import os
|
||||
|
||||
|
||||
def _sensor_worker(sensor_id, config, data_queue, stop_event, use_async=False):
|
||||
def _sensor_worker(sensor_id, config, data_queue, stop_event, compute_contact=False):
|
||||
"""
|
||||
Worker function that runs in a separate PROCESS for each sensor.
|
||||
Each process has its own Python interpreter and CUDA context.
|
||||
|
||||
Args:
|
||||
use_async: If True, use 3-stage async pipeline within the process
|
||||
"""
|
||||
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
|
||||
|
||||
from .sensor import Sensor
|
||||
|
||||
print(f"[Sensor {sensor_id}] Process started (PID: {os.getpid()}), async={use_async}")
|
||||
print(f"[Sensor {sensor_id}] Process started (PID: {os.getpid()})")
|
||||
|
||||
sensor = Sensor(
|
||||
vid_src=config.get('vid_src'),
|
||||
@@ -32,10 +29,6 @@ def _sensor_worker(sensor_id, config, data_queue, stop_event, use_async=False):
|
||||
cal_path=config.get('cal_path', None)
|
||||
)
|
||||
|
||||
# Enable async pipeline if requested
|
||||
if use_async:
|
||||
sensor.enable_async(queue_size=2)
|
||||
|
||||
frame_count = 0
|
||||
|
||||
def queue_put(data):
|
||||
@@ -48,43 +41,76 @@ def _sensor_worker(sensor_id, config, data_queue, stop_event, use_async=False):
|
||||
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
if use_async:
|
||||
# Use async get
|
||||
result = sensor.get_async(timeout=0.05)
|
||||
if result is None:
|
||||
continue
|
||||
data = {
|
||||
'sensor_id': sensor_id,
|
||||
'img': result['img'].copy() if result['img'] is not None else None,
|
||||
'flow': result['flow'].copy() if result['flow'] is not None else None,
|
||||
'normal': result['normal'].copy() if result['normal'] is not None else None,
|
||||
'shear': result['shear'].copy() if result['shear'] is not None else None,
|
||||
'depth': result['depth'].copy() if result['depth'] is not None else None,
|
||||
'fps': result['fps'],
|
||||
'fnormal': result['fnormal'],
|
||||
'fshearx': result['fshearx'],
|
||||
'fsheary': result['fsheary'],
|
||||
'ftor': result['ftor'],
|
||||
'frame_count': frame_count,
|
||||
}
|
||||
# Sync mode
|
||||
sensor.get_img()
|
||||
sensor.compute_deformation()
|
||||
if compute_contact:
|
||||
sensor.compute_contact()
|
||||
|
||||
# Use read_info to get all sensor data
|
||||
|
||||
img, flow, normal, shear, fps, fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.IMG,
|
||||
sensor.info.VRAW,
|
||||
sensor.info.VNORMAL,
|
||||
sensor.info.VSHEAR,
|
||||
sensor.info.FPS,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY
|
||||
)
|
||||
if compute_contact:
|
||||
depth, centroid, contour = sensor.read_info(
|
||||
sensor.info.DEPTH,
|
||||
sensor.info.CENTROID,
|
||||
sensor.info.CONTOUR
|
||||
)
|
||||
else:
|
||||
# Sync mode
|
||||
sensor.get_img()
|
||||
sensor.compute_deformation()
|
||||
data = {
|
||||
'sensor_id': sensor_id,
|
||||
'img': sensor.img.copy() if sensor.img is not None else None,
|
||||
'flow': sensor.flow.copy() if sensor.flow is not None else None,
|
||||
'normal': sensor.vnormal.copy() if sensor.vnormal is not None else None,
|
||||
'shear': sensor.shear.copy() if sensor.shear is not None else None,
|
||||
'depth': sensor.depth.copy() if sensor.depth is not None else None,
|
||||
'fps': sensor.fps,
|
||||
'fnormal': sensor.fnormal,
|
||||
'fshearx': sensor.fshearx,
|
||||
'fsheary': sensor.fsheary,
|
||||
'ftor': sensor.ftor,
|
||||
'frame_count': frame_count,
|
||||
}
|
||||
depth = None
|
||||
centroid = None
|
||||
contour = None
|
||||
|
||||
# Build data dictionary
|
||||
data = {
|
||||
'sensor_id': sensor_id,
|
||||
'img': img if img is not None and img.size > 0 else None,
|
||||
'flow': flow if flow is not None and flow.size > 0 else None,
|
||||
'normal': normal if normal is not None and normal.size > 0 else None,
|
||||
'shear': shear if shear is not None and shear.size > 0 else None,
|
||||
'fps': fps,
|
||||
'fnormal': fnormal,
|
||||
'fshearx': fshearx,
|
||||
'fsheary': fsheary,
|
||||
'frame_count': frame_count,
|
||||
}
|
||||
|
||||
# Handle contact-related fields
|
||||
if compute_contact:
|
||||
# depth and depth_map
|
||||
data['depth'] = depth if depth is not None and depth.size > 0 else None
|
||||
data['depth_map'] = data['depth'] # Alias for compatibility
|
||||
|
||||
# centroid - can be None or numpy array
|
||||
if centroid is not None:
|
||||
# Check if it's a valid array with elements
|
||||
if hasattr(centroid, 'size') and centroid.size > 0:
|
||||
data['centroid'] = centroid
|
||||
data['centroid_found'] = True
|
||||
else:
|
||||
data['centroid'] = None
|
||||
data['centroid_found'] = False
|
||||
else:
|
||||
data['centroid'] = None
|
||||
data['centroid_found'] = False
|
||||
|
||||
# contour
|
||||
data['contour'] = contour if contour is not None and contour.size > 0 else None
|
||||
else:
|
||||
data['depth'] = None
|
||||
data['depth_map'] = None
|
||||
data['centroid'] = None
|
||||
data['centroid_found'] = False
|
||||
data['contour'] = None
|
||||
|
||||
queue_put(data)
|
||||
frame_count += 1
|
||||
@@ -130,7 +156,7 @@ class MultiSensor:
|
||||
multi.stop()
|
||||
"""
|
||||
|
||||
def __init__(self, sensor_configs):
|
||||
def __init__(self, sensor_configs, compute_contact=False):
|
||||
"""
|
||||
Initialize MultiSensor manager.
|
||||
|
||||
@@ -155,7 +181,7 @@ class MultiSensor:
|
||||
self.sensor_configs = sensor_configs
|
||||
self.num_sensors = len(sensor_configs)
|
||||
self._mp = mp
|
||||
self._use_async = False
|
||||
self.compute_contact = compute_contact
|
||||
|
||||
self.processes = []
|
||||
self.data_queues = []
|
||||
@@ -167,33 +193,24 @@ class MultiSensor:
|
||||
self.data_queues.append(mp.Queue(maxsize=2))
|
||||
self.stop_events.append(mp.Event())
|
||||
|
||||
def start(self, use_async=False):
|
||||
"""
|
||||
Start all sensor worker processes.
|
||||
|
||||
Args:
|
||||
use_async: If True, each sensor process uses 3-stage async pipeline
|
||||
for maximum throughput (Capture → Flow → HHD in parallel)
|
||||
"""
|
||||
def start(self):
|
||||
"""Start all sensor worker processes."""
|
||||
if self._started:
|
||||
print("MultiSensor already started")
|
||||
return
|
||||
|
||||
self._use_async = use_async
|
||||
|
||||
for i, config in enumerate(self.sensor_configs):
|
||||
process = self._mp.Process(
|
||||
target=_sensor_worker,
|
||||
args=(i, config, self.data_queues[i], self.stop_events[i], use_async),
|
||||
args=(i, config, self.data_queues[i], self.stop_events[i], self.compute_contact),
|
||||
daemon=True
|
||||
)
|
||||
process.start()
|
||||
self.processes.append(process)
|
||||
print(f"Started sensor {i} (PID: {process.pid})")
|
||||
|
||||
mode = "async (3-stage pipeline)" if use_async else "sync"
|
||||
self._started = True
|
||||
print(f"All {len(self.processes)} sensors started in {mode} mode")
|
||||
print(f"All {len(self.processes)} sensors started")
|
||||
|
||||
def get(self, sensor_idx, timeout=0.01):
|
||||
"""
|
||||
@@ -283,8 +300,8 @@ class MultiSensor:
|
||||
print("All sensors stopped.")
|
||||
|
||||
def __enter__(self):
|
||||
"""Context manager entry - starts sensors (sync mode by default)."""
|
||||
self.start(use_async=False)
|
||||
"""Context manager entry - starts sensors."""
|
||||
self.start()
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
@@ -295,23 +312,4 @@ class MultiSensor:
|
||||
def __del__(self):
|
||||
"""Cleanup on deletion."""
|
||||
self.stop()
|
||||
|
||||
@classmethod
|
||||
def async_context(cls, sensor_configs):
|
||||
"""
|
||||
Create a MultiSensor that uses async mode when used as context manager.
|
||||
|
||||
Example:
|
||||
with MultiSensor.async_context(configs) as multi:
|
||||
for all_data in multi:
|
||||
...
|
||||
"""
|
||||
return _AsyncMultiSensorContext(sensor_configs)
|
||||
|
||||
|
||||
class _AsyncMultiSensorContext(MultiSensor):
|
||||
"""Helper class for async context manager."""
|
||||
def __enter__(self):
|
||||
self.start(use_async=True)
|
||||
return self
|
||||
|
||||
|
||||
Reference in New Issue
Block a user