init
This commit is contained in:
cheng
@@ -0,0 +1,307 @@
|
||||
import os
|
||||
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
|
||||
|
||||
import argparse
|
||||
import multiprocessing as mp
|
||||
import queue
|
||||
import time
|
||||
from multiprocessing import Process, Queue
|
||||
|
||||
from .paths import ensure_project_paths
|
||||
|
||||
ensure_project_paths()
|
||||
|
||||
|
||||
def parse_video_source(value):
|
||||
if isinstance(value, str):
|
||||
value = value.strip()
|
||||
if value.isdigit():
|
||||
return int(value)
|
||||
return value
|
||||
|
||||
|
||||
def _put_latest(result_queue, data):
|
||||
try:
|
||||
result_queue.put_nowait(data)
|
||||
return
|
||||
except queue.Full:
|
||||
pass
|
||||
|
||||
try:
|
||||
result_queue.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
result_queue.put_nowait(data)
|
||||
except queue.Full:
|
||||
pass
|
||||
|
||||
|
||||
def _sensor_force_worker(
|
||||
sensor_id,
|
||||
vid_src,
|
||||
config_name,
|
||||
rotation_config_path,
|
||||
result_queue,
|
||||
stop_event,
|
||||
target_fps,
|
||||
cuda,
|
||||
isstitch,
|
||||
backend,
|
||||
motion_threshold,
|
||||
):
|
||||
ensure_project_paths()
|
||||
import orisys
|
||||
|
||||
sensor = None
|
||||
try:
|
||||
sensor = orisys.Sensor(
|
||||
vid_src,
|
||||
isstitch=isstitch,
|
||||
cuda=cuda,
|
||||
verbose=False,
|
||||
config_name=config_name,
|
||||
backend=backend,
|
||||
rotation_config_path=rotation_config_path,
|
||||
)
|
||||
|
||||
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()
|
||||
img = sensor.get_img()
|
||||
if img is None:
|
||||
time.sleep(0.01)
|
||||
continue
|
||||
|
||||
is_contact = sensor.compute_deformation(
|
||||
check_motion=True,
|
||||
threshold=motion_threshold,
|
||||
)
|
||||
fps, fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FPS,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
|
||||
_put_latest(
|
||||
result_queue,
|
||||
{
|
||||
"sensor_id": sensor_id,
|
||||
"fnormal": float(fnormal),
|
||||
"fshearx": float(fshearx),
|
||||
"fsheary": float(fsheary),
|
||||
"fps": float(fps),
|
||||
"is_contact": bool(is_contact),
|
||||
"timestamp": time.time(),
|
||||
},
|
||||
)
|
||||
|
||||
if frame_interval > 0:
|
||||
sleep_time = frame_interval - (time.perf_counter() - t_start)
|
||||
if sleep_time > 0.001:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
except Exception as exc:
|
||||
_put_latest(
|
||||
result_queue,
|
||||
{
|
||||
"sensor_id": sensor_id,
|
||||
"error": str(exc),
|
||||
"timestamp": time.time(),
|
||||
},
|
||||
)
|
||||
finally:
|
||||
if sensor is not None:
|
||||
try:
|
||||
sensor.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class TwoSensorForceReader:
|
||||
def __init__(
|
||||
self,
|
||||
video1=0,
|
||||
video2=1,
|
||||
config1="./config/ddjx01.json",
|
||||
config2="./config/ddjx01.json",
|
||||
rotation1="./config/rotation_config_0.json",
|
||||
rotation2="./config/rotation_config_1.json",
|
||||
target_fps=30,
|
||||
cuda=True,
|
||||
isstitch=True,
|
||||
backend="auto",
|
||||
motion_threshold=0,
|
||||
queue_size=5,
|
||||
):
|
||||
self.sensor_configs = [
|
||||
{
|
||||
"sensor_id": 0,
|
||||
"vid_src": parse_video_source(video1),
|
||||
"config_name": config1,
|
||||
"rotation_config_path": rotation1,
|
||||
},
|
||||
{
|
||||
"sensor_id": 1,
|
||||
"vid_src": parse_video_source(video2),
|
||||
"config_name": config2,
|
||||
"rotation_config_path": rotation2,
|
||||
},
|
||||
]
|
||||
self.target_fps = target_fps
|
||||
self.cuda = cuda
|
||||
self.isstitch = isstitch
|
||||
self.backend = backend
|
||||
self.motion_threshold = motion_threshold
|
||||
self.queue_size = queue_size
|
||||
|
||||
self.result_queues = []
|
||||
self.stop_events = []
|
||||
self.processes = []
|
||||
self.latest = [None, None]
|
||||
|
||||
def start(self):
|
||||
if self.processes:
|
||||
return
|
||||
|
||||
self.result_queues = [Queue(maxsize=self.queue_size) for _ in self.sensor_configs]
|
||||
self.stop_events = [mp.Event() for _ in self.sensor_configs]
|
||||
self.latest = [None] * len(self.sensor_configs)
|
||||
|
||||
for idx, config in enumerate(self.sensor_configs):
|
||||
process = Process(
|
||||
target=_sensor_force_worker,
|
||||
args=(
|
||||
config["sensor_id"],
|
||||
config["vid_src"],
|
||||
config["config_name"],
|
||||
config["rotation_config_path"],
|
||||
self.result_queues[idx],
|
||||
self.stop_events[idx],
|
||||
self.target_fps,
|
||||
self.cuda,
|
||||
self.isstitch,
|
||||
self.backend,
|
||||
self.motion_threshold,
|
||||
),
|
||||
)
|
||||
process.start()
|
||||
self.processes.append(process)
|
||||
|
||||
def update(self):
|
||||
for idx, result_queue in enumerate(self.result_queues):
|
||||
data = None
|
||||
while True:
|
||||
try:
|
||||
data = result_queue.get_nowait()
|
||||
except queue.Empty:
|
||||
break
|
||||
except Exception:
|
||||
break
|
||||
|
||||
if data is not None:
|
||||
if "error" in data:
|
||||
raise RuntimeError(
|
||||
f"sensor {data.get('sensor_id', idx)} failed: {data['error']}"
|
||||
)
|
||||
self.latest[idx] = data
|
||||
|
||||
return self.latest
|
||||
|
||||
def get_samples(self, timeout=0.0):
|
||||
deadline = time.perf_counter() + max(0.0, float(timeout))
|
||||
while True:
|
||||
self.update()
|
||||
if all(sample is not None for sample in self.latest):
|
||||
return self.latest[0], self.latest[1]
|
||||
if time.perf_counter() >= deadline:
|
||||
return self.latest[0], self.latest[1]
|
||||
time.sleep(0.005)
|
||||
|
||||
def get_forces(self, timeout=0.0):
|
||||
left, right = self.get_samples(timeout=timeout)
|
||||
left_force = None if left is None else left["fnormal"]
|
||||
right_force = None if right is None else right["fnormal"]
|
||||
return left_force, right_force
|
||||
|
||||
def stop(self):
|
||||
for event in self.stop_events:
|
||||
event.set()
|
||||
|
||||
for process in self.processes:
|
||||
process.join(timeout=2.0)
|
||||
if process.is_alive():
|
||||
process.terminate()
|
||||
process.join(timeout=1.0)
|
||||
|
||||
self.processes = []
|
||||
self.stop_events = []
|
||||
self.result_queues = []
|
||||
|
||||
def __enter__(self):
|
||||
self.start()
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc, traceback):
|
||||
self.stop()
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--video1", "-v1", type=str, default="0")
|
||||
parser.add_argument("--video2", "-v2", type=str, default="1")
|
||||
parser.add_argument("--config1", type=str, default="./config/ddjx01.json")
|
||||
parser.add_argument("--config2", type=str, default="./config/ddjx01.json")
|
||||
parser.add_argument("--rotation1", "-r1", type=str, default="./config/rotation_config_0.json")
|
||||
parser.add_argument("--rotation2", "-r2", type=str, default="./config/rotation_config_1.json")
|
||||
parser.add_argument("--target-fps", type=float, default=30)
|
||||
parser.add_argument("--motion-threshold", type=float, default=0)
|
||||
parser.add_argument("--cpu", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
reader = TwoSensorForceReader(
|
||||
video1=args.video1,
|
||||
video2=args.video2,
|
||||
config1=args.config1,
|
||||
config2=args.config2,
|
||||
rotation1=args.rotation1,
|
||||
rotation2=args.rotation2,
|
||||
target_fps=args.target_fps,
|
||||
cuda=not args.cpu,
|
||||
motion_threshold=args.motion_threshold,
|
||||
)
|
||||
|
||||
print("Starting two-sensor force reader. Press Ctrl+C to stop.")
|
||||
reader.start()
|
||||
try:
|
||||
while True:
|
||||
left_sample, right_sample = reader.get_samples(timeout=1.0)
|
||||
left_force, right_force = reader.get_forces()
|
||||
left_fps = 0.0 if left_sample is None else left_sample["fps"]
|
||||
right_fps = 0.0 if right_sample is None else right_sample["fps"]
|
||||
left_contact = False if left_sample is None else left_sample["is_contact"]
|
||||
right_contact = False if right_sample is None else right_sample["is_contact"]
|
||||
|
||||
print(
|
||||
"left={:>10} right={:>10} fps=({:5.1f}, {:5.1f}) contact=({}, {})".format(
|
||||
"None" if left_force is None else f"{left_force:.4f}",
|
||||
"None" if right_force is None else f"{right_force:.4f}",
|
||||
left_fps,
|
||||
right_fps,
|
||||
int(left_contact),
|
||||
int(right_contact),
|
||||
)
|
||||
)
|
||||
time.sleep(0.05)
|
||||
except KeyboardInterrupt:
|
||||
print("\nStopping.")
|
||||
finally:
|
||||
reader.stop()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
mp.freeze_support()
|
||||
main()
|
||||
Reference in New Issue
Block a user