diff --git a/py_workspace/.gitignore b/py_workspace/.gitignore index 5e65f6b..6535892 100644 --- a/py_workspace/.gitignore +++ b/py_workspace/.gitignore @@ -218,3 +218,4 @@ __marimo__/ .venv *.svo2 .ruff_cache +output/ diff --git a/py_workspace/depth_sensing.py b/py_workspace/depth_sensing.py index f3b7f78..9491494 100644 --- a/py_workspace/depth_sensing.py +++ b/py_workspace/depth_sensing.py @@ -1,23 +1,3 @@ -######################################################################## -# -# Copyright (c) 2022, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - """ This sample demonstrates how to capture a live 3D point cloud with the ZED SDK and display the result in an OpenGL window. diff --git a/py_workspace/recording_multi.py b/py_workspace/recording_multi.py index 90daf44..9f054b1 100755 --- a/py_workspace/recording_multi.py +++ b/py_workspace/recording_multi.py @@ -8,7 +8,11 @@ import threading import signal import time import sys +import click import zed_network_utils +import cv2 +import queue +import os # Global variable to handle exit exit_app = False @@ -21,15 +25,27 @@ def signal_handler(signal, frame): print("\nCtrl+C pressed. Exiting...") -def acquisition(zed): +def acquisition(zed, frame_queue=None): """Acquisition thread function to continuously grab frames""" infos = zed.get_camera_information() + mat = sl.Mat() while not exit_app: if zed.grab() == sl.ERROR_CODE.SUCCESS: - # If needed, add more processing here - # But be aware that any processing involving the GiL will slow down the multi threading performance - pass + if frame_queue is not None: + # Retrieve left image + zed.retrieve_image(mat, sl.VIEW.LEFT) + # Convert to numpy and copy to ensure thread safety when passing to main + try: + # Keep latest frame only + if frame_queue.full(): + try: + frame_queue.get_nowait() + except queue.Empty: + pass + frame_queue.put_nowait(mat.get_data().copy()) + except queue.Full: + pass print(f"{infos.camera_model}[{infos.serial_number}] QUIT") @@ -39,8 +55,8 @@ def acquisition(zed): zed.close() -def open_camera(zed, config): - """Open a camera with given configuration and enable streaming""" +def open_camera(zed, config, save_dir): + """Open a camera with given configuration and enable recording""" ip, port = zed_network_utils.extract_ip_port(config) if not ip or not port: @@ -59,7 +75,7 @@ def open_camera(zed, config): print(f"ZED SN{serial} Opened from {ip}:{port}") # Enable Recording - output_svo_file = f"ZED_SN{serial}.svo2" + output_svo_file = os.path.join(save_dir, f"ZED_SN{serial}.svo2") recording_param = sl.RecordingParameters( output_svo_file.replace(" ", ""), sl.SVO_COMPRESSION_MODE.H265 ) @@ -76,41 +92,63 @@ def open_camera(zed, config): return True -def main(): +@click.command() +@click.option( + "--monitor", is_flag=True, help="Enable local monitoring of the camera streams." +) +@click.option( + "--config", + default=zed_network_utils.DEFAULT_CONFIG_PATH, + help="Path to the network configuration JSON file.", + type=click.Path(exists=True), +) +@click.option( + "--save-dir", + default=os.getcwd(), + help="Directory where SVO files will be saved.", + type=click.Path(exists=True, file_okay=False, dir_okay=True, writable=True), +) +def main(monitor, config, save_dir): global exit_app # Read network configuration using utility - network_config = zed_network_utils.parse_network_config() + network_config = zed_network_utils.parse_network_config(config) if not network_config: - return 1 + return print(f"Found {len(network_config)} cameras in configuration") if len(network_config) == 0: print("No ZED configured, exit program") - return 1 + return zed_open = False # Open all cameras zeds = [] threads = [] + queues = {} # serial -> queue for serial, config in network_config.items(): zed = sl.Camera() - if open_camera(zed, config): + if open_camera(zed, config, save_dir): zeds.append(zed) zed_open = True + fq = None + if monitor: + fq = queue.Queue(maxsize=1) + queues[serial] = fq + # Start acquisition thread immediately - thread = threading.Thread(target=acquisition, args=(zed,)) + thread = threading.Thread(target=acquisition, args=(zed, fq)) thread.start() threads.append(thread) if not zed_open: print("No ZED opened, exit program") - return 1 + return # Set up signal handler for Ctrl+C signal.signal(signal.SIGINT, signal_handler) @@ -118,10 +156,30 @@ def main(): # Main loop while not exit_app: - time.sleep(0.02) + if monitor: + for serial, q in queues.items(): + try: + frame = q.get_nowait() + # Display the frame + # Use serial number as window name to distinguish cameras + # Resize is handled by window automatically usually, or we can resize + cv2.imshow(f"ZED {serial}", frame) + except queue.Empty: + pass + + # Check for quit key + key = cv2.waitKey(10) + if key == 113 or key == ord("q") or key == 27: # q or Esc + exit_app = True + else: + time.sleep(0.02) # Wait for all threads to finish print("Exit signal, closing ZEDs") + + if monitor: + cv2.destroyAllWindows() + time.sleep(0.1) for thread in threads: @@ -132,4 +190,4 @@ def main(): if __name__ == "__main__": - sys.exit(main()) + main() diff --git a/py_workspace/streaming_receiver.py b/py_workspace/streaming_receiver.py index 3e716df..7e180fd 100755 --- a/py_workspace/streaming_receiver.py +++ b/py_workspace/streaming_receiver.py @@ -1,23 +1,3 @@ -######################################################################## -# -# Copyright (c) 2022, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - """ Read a stream and display the left images using OpenCV """ diff --git a/py_workspace/svo_playback.py b/py_workspace/svo_playback.py index e644c2e..03a7cbc 100755 --- a/py_workspace/svo_playback.py +++ b/py_workspace/svo_playback.py @@ -1,10 +1,10 @@ -# ... existing code ... import sys import pyzed.sl as sl import cv2 import argparse import os import math +from pathlib import Path def progress_bar(percent_done, bar_length=50): @@ -16,8 +16,38 @@ def progress_bar(percent_done, bar_length=50): def main(opt): - svo_files = opt.input_svo_files + input_paths = [Path(p) for p in opt.input_svo_files] + svo_files = [] + + for path in input_paths: + if path.is_dir(): + print(f"Searching for SVO files in {path}...") + found = sorted( + [ + str(f) + for f in path.iterdir() + if f.is_file() and f.suffix.lower() in (".svo", ".svo2") + ] + ) + if found: + print(f"Found {len(found)} files in {path}") + svo_files.extend(found) + else: + print(f"No .svo or .svo2 files found in {path}") + elif path.is_file(): + svo_files.append(str(path)) + else: + print(f"Path not found: {path}") + + if not svo_files: + print("No valid SVO files provided. Exiting.") + return + + # Sort files to ensure deterministic order + svo_files.sort() + cameras = [] + cam_data = [] # List of dicts to store camera info print(f"Opening {len(svo_files)} SVO files...") @@ -177,7 +207,7 @@ if __name__ == "__main__": "--input_svo_files", nargs="+", type=str, - help="Path to .svo/.svo2 files", + help="Path to .svo/.svo2 files or directories", required=True, ) opt = parser.parse_args()