Add multi-camera sync tool and readme

docs/multi_camera_syned.MD

@@ -0,0 +1,106 @@
+## Multi-camera synchronous image storage and script analysis
+
+This section describes how to use multiple Orbbec cameras to store images synchronously in a ROS 2 environment and use scripts to parse the data.
+
+### Connection preparation
+
+Please follow the instructions to connect multiple Orbbec cameras and Multi-Camera Sync Hub Pro.
+
+* For detailed guidance on Multi-Camera Sync Hub Pro, you can refer to [Set up cameras for external synchronization](https://www.orbbec.com/docs/set-up-cameras-for-external-synchronization_v1-2/).
+* Example of connecting multiple cameras to Multi-Camera Sync Hub Pro.
+
+![Schematic diagram of multi-machine synchronizer connection](./images/image7.jpg)
+
+### Multi-camera synchronous image storage
+
+#### Check the USB port of your device
+
+```bash
+ros2 run orbbec_camera list_devices_node
+```
+
+![Check the USB port](./images/image8.jpg)
+
+#### **Parameter configuration**
+
+##### multi_camera_synced.launch.py
+
+1. `usb_port` is set to the USB port of the device.
+2. `device_num` is set to 2, indicating that two cameras are started.
+3. `sync_mode` is set to primary, indicating that the camera device is set to host mode.
+4. The slave sets `trigger_out_enabled` to false.
+
+![Launch Configuration](./images/image9.jpg)
+
+##### multi_save_rgbir_params.json
+
+Configure USB ports and names for multiple devices.
+
+![Json Configuration](./images/image10.jpg)
+
+##### camera_params.yaml
+
+Enable the left IR camera.
+
+`enable_left_ir: true`
+
+##### start_multi_sync.sh
+
+`{data: 100}` means that each stream of each camera stores 100 images.
+
+![Shell Configuration](./images/image11.jpg)
+
+#### Running
+
+Start multiple Orbbec cameras (select 1).
+
+* On terminal 1
+
+```bash
+cd  ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/tools
+sudo chmod +x start_multi_sync.sh
+bash start_multi_sync.sh
+```
+
+![Startup Script](./images/image12.jpg)
+
+Save the image synchronously (select 4).
+
+* On terminal 2
+
+```bash
+cd  ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/tools
+bash start_multi_sync.sh
+```
+
+### Script parsing
+
+`multicamera_sync` - save the image and analysis script storage directory.
+
+* `ouput` - save the image directory
+* `Python` - analysis script directory
+
+Note: `multicamera_sync` needs to be copied to the current workspace directory (same level as the `install` directory).
+
+#### Configuration Files
+
+* `Config.ini` configures the camera's actual frame rate and maximum error timestamp threshold.
+* `DevicesInfo.txt` configures the number of cameras and the camera's pid, vid, SN number and other parameters.
+* `StreamProfileInfo.txt` configures the camera resolution, frame rate, and encoding format.
+
+Note: `DevicesInfo.txt` and `StreamProfileInfo.txt` are files required by the parsing script and need to be copied to the same directory as the `TotalModeFrames` folder (The `TotalModeFrames` directory is created by the image saving script and is used to store image data collected synchronously by multiple cameras).
+
+#### Running
+
+Run the parsing script to parse the images stored synchronously by multiple cameras.
+
+```bash
+cd  ~/ros2_ws/multicamera_sync/Python
+python3 SyncFramesMain.py
+```
+
+#### Output
+
+* `matchFrames`: Incomplete dataset of the same camera group, camera image information is missing.
+* `notMatchFrames`: Complete dataset of the same camera group.
+* `abnormal`: The same set of complete data sets of cameras, and the camera synchronization time exceeds the parameter `tspRangeThreshold` set in `Config.ini`.

docs/multi_camera_syned_CN.MD

@@ -0,0 +1,106 @@
+## 多机同步存图及脚本解析
+
+本节介绍如何在ROS 2环境中使用多个Orbbec摄像头同步存图，并使用脚本解析数据。
+
+### 连接准备
+
+请根据指导连接多个Orbbec摄像头和多机同步器。
+
+* 有关多机同步器的详细指导，请参阅[设置相机进行外部同步](https://www.orbbec.com/docs/set-up-cameras-for-external-synchronization_v1-2/)。
+* 多个Orbbec摄像头和多机同步器的连接示例。
+
+![多机同步器连接示意图](./images/image7.jpg)
+
+### 多机同步存图
+
+#### 查看设备的USB端口
+
+```bash
+ros2 run orbbec_camera list_devices_node
+```
+
+![查看设备的USB端口](./images/image8.jpg)
+
+#### 参数配置
+
+##### multi_camera_synced.launch.py
+
+1. `usb_port` 设置为设备的USB端口。
+2. `device_num` 设置为2，说明启动了两个相机。
+3. `sync_mode` 设置为 primary，说明相机设备设置为主机模式。
+4. 从机设置 `trigger_out_enabled` 为 false。
+
+![launch配置](./images/image9.jpg)
+
+##### multi_save_rgbir_params.json
+
+配置多个设备的USB端口和名称。
+
+![json配置](./images/image10.jpg)
+
+##### camera_params.yaml
+
+启用左IR摄像头。
+
+`enable_left_ir: true`
+
+##### start_multi_sync.sh
+
+`{data: 100}` 表示每台相机的每个流存储100张图片。
+
+![脚本配置](./images/image11.jpg)
+
+#### 启动多机同步存图
+
+启动多个Orbbec摄像头（选择1）。
+
+* 终端1
+
+```bash
+cd  ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/tools
+sudo chmod +x start_multi_sync.sh
+bash start_multi_sync.sh
+```
+
+![启动脚本](./images/image12.jpg)
+
+同步保存图片（选择4）。
+
+* 终端2
+
+```bash
+cd  ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/tools
+bash start_multi_sync.sh
+```
+
+### 脚本解析
+
+`multicamera_sync` - 保存图片以及解析脚本存放目录。
+
+* `ouput` - 保存图片目录
+* `Python` - 解析脚本目录
+
+注意：`multicamera_sync` 需要复制到当前工作空间目录（与 `install` 目录同级）。
+
+#### 配置文件
+
+* `Config.ini` 配置相机实际帧率和最大误差时间戳阈值。
+* `DevicesInfo.txt` 配置相机数量以及相机的pid、vid、SN号等参数。
+* `StreamProfileInfo.txt` 配置相机分辨率、帧率以及编码格式。
+
+注意：`DevicesInfo.txt` 和 `StreamProfileInfo.txt` 是解析脚本所需要的文件，需要复制到 `TotalModeFrames` 文件夹同级目录（ `TotalModeFrames` 目录由存图脚本创建，用于存储多相机同步采集的图像数据）。
+
+#### 运行脚本
+
+运行解析脚本解析多机同步存储的图片。
+
+```bash
+cd  ~/ros2_ws/multicamera_sync/Python
+python3 SyncFramesMain.py
+```
+
+#### 输出结果
+
+* `matchFrames`：同一组摄像头不完整的数据集，存在摄像头图片信息缺失。
+* `notMatchFrames`：同一组摄像头完整的数据集。
+* `abnormal`：同一组摄像头完整的数据集，且相机同步时间超过 `Config.ini` 设置的参数 `tspRangeThreshold`。

orbbec_camera/config/multicamera_sync/DevicesInfo.txt

@@ -0,0 +1,42 @@
+{
+	"deviceVid":	"0x2BC5",
+	"devicePid":	"0x0800",
+	"primarySerialNumber":	"CP1S34D0004J",
+	"devices":	[{
+			"index":	0,
+			"isPrimaryDevice":	true,
+			"vid":	"0x2BC5",
+			"pid":	"0x0800",
+			"name":	"Orbbec Gemini 335",
+			"firmwareVersion":	"1.3.64",
+			"serialNumber":	"CP1S34D0004J",
+			"hardwareVersion":	"0.1",
+			"ipAddress":	"0.0.0.0",
+			"extensionInfo":	{
+				"ExtensionInfo":	{
+					"IspFwVer":	"20240820",
+					"IspNeedVer":	"20240820",
+					"HwType":	"R1",
+					"HwVer":	"V1.1"
+				}
+			}
+		}, {
+			"index":	1,
+			"isPrimaryDevice":	false,
+			"vid":	"0x2BC5",
+			"pid":	"0x0800",
+			"name":	"Orbbec Gemini 335",
+			"firmwareVersion":	"1.3.64",
+			"serialNumber":	"CP1S34D00038",
+			"hardwareVersion":	"0.1",
+			"ipAddress":	"0.0.0.0",
+			"extensionInfo":	{
+				"ExtensionInfo":	{
+					"IspFwVer":	"20240820",
+					"IspNeedVer":	"20240820",
+					"HwType":	"R1",
+					"HwVer":	"V1.0"
+				}
+			}
+		}]
+}

orbbec_camera/config/multicamera_sync/Python/Config.ini

@@ -0,0 +1,5 @@
+[Parameter]
+# 单位：FPS,请依据数据集实际帧率填写，否则会导致匹配不准确
+frameRate=60
+# 单位：ms，当匹配后某组数据帧的时间戳极差大于等于tspRangeThreshold，文件名会增加标注
+tspRangeThreshold=4

orbbec_camera/config/multicamera_sync/Python/SyncFramesMain.py

@@ -0,0 +1,97 @@
+import os
+import subprocess
+import json
+import platform
+
+def list_subdirectories(path):
+    # 遍历指定目录
+    for f in os.listdir(path):
+        # 判断是否是子目录
+        if os.path.isdir(os.path.join(path, f)):
+            yield f
+
+def read_pid_from_device_json(frames_dir):
+    file_path = frames_dir + "/DevicesInfo.txt"
+    if not os.path.exists(file_path):
+        print(file_path + " not exists. Please check it")
+        return ""
+
+    with open(file_path,  'r') as f:
+        data = json.load(f)
+
+    if not 'devicePid' in data:
+        print("Not found 'devicePid' in " + file_path)
+        return ""
+
+    return data['devicePid']
+
+def execute_sync_width_pid(frames_dir, pid):
+    sync_script_file=""
+    if pid.lower() == "0x0675":
+        sync_script_file="script/config_frameMatch_Gemini2VL.py"
+    elif pid.lower() == "0x0670" or pid.lower() == "0x0701":
+        sync_script_file="script/config_frameMatch-systemTimestamp.py"
+    elif pid.lower() == "0x0660":
+        sync_script_file="script/config_frameMatch-Astra2_new.py"
+    elif pid.lower() == "0x0800" or pid.lower() == "0x0804" or pid.lower() == "0x0803" or pid.lower() == "0x0807" or pid.lower() == "0x0801" or pid.lower() == "0x0805" or pid.lower() == "0x080b" or pid.lower() == "0x080e" or pid.lower() == "0x080f" or pid.lower() == "0x0A13":
+        sync_script_file="script/config_frameMatch_g330.py"
+    else:
+        sync_script_file="script/config_frameMatch.py"
+    
+    if len(sync_script_file) > 0:
+        print(f"execute synchronize frames. script_file={sync_script_file}, frames_dir={frames_dir}")
+        if platform.system() == "Windows": 
+            output = subprocess.check_output(["python", sync_script_file, os.path.abspath(f"{frames_dir}"), os.getcwd()])
+            print(output.decode("gbk"))
+        else:
+            output = subprocess.check_output(["python3", sync_script_file, os.path.abspath(f"{frames_dir}"), os.getcwd()])
+            print(output.decode("utf-8"))
+    else:
+        print("Invalid pid=" + pid + ", not match sync frame script")
+
+def sync_frames(frames_dir):
+    pid = read_pid_from_device_json(frames_dir)
+    if len(pid) <= 0:
+        print("Get pid failed.")
+        return
+    
+    execute_sync_width_pid(frames_dir, pid)
+
+def main():
+    # 指定目录路径
+    frames_output_path = os.path.abspath("../output")
+
+    # 获取所有子目录
+    subdirectories = list(list_subdirectories(frames_output_path))
+
+    if not subdirectories:
+        print("No subdirectories found in the specified directory.")
+
+    # 打印子目录列表并让用户选择
+    for i, subdir in enumerate(subdirectories):
+        print(f"{i + 1}. {subdir}")
+
+    # 获取用户输入
+    choice = input("Please select a subdirectory by number (or 'q' to quit): ")
+
+    if choice.lower() == 'q':
+        return
+
+    # 验证用户输入
+    is_index_valid = False
+    try:
+        index = int(choice) - 1
+        if 0 <= index < len(subdirectories):
+            is_index_valid = True
+        else:
+            print(f"Invalid choice. index={index}. Please enter a number between 1 and the total number of subdirectories.")
+    except ValueError:
+        print("Invalid choice(ValueException). Please enter a number between 1 and the total number of subdirectories")
+    if is_index_valid:
+        selected_subdir = subdirectories[index]
+        print(f"You selected the subdirectory: {selected_subdir}")
+        sync_frames(f"{frames_output_path }/{selected_subdir}")
+
+
+if __name__ == "__main__":
+    main()

orbbec_camera/config/multicamera_sync/Python/script/config_frameMatch-Astra2_new.py

@@ -0,0 +1,385 @@
+import os.path
+import argparse
+import sys
+import json
+import shutil
+import configparser
+
+from pathlib import Path
+from datetime import datetime
+
+## Config.ini中配置
+# 单位：FPS,请依据数据集实际帧率填写，否则会导致匹配不准确
+frameRate = -1
+# 单位：ms，当匹配后某组数据帧的时间戳极差大于等于tspRangeThreshold，文件名会增加标注
+tspRangeThreshold = -1
+
+## Python脚本自动解析, 不要修改
+primaryId = '-1' #主机ID
+mainPythonWorkPath = "" # SyncFramesMain.py的工作目录
+sourceFramesPath = ""  # 请勿修改，固定值,TotalMode目录路径
+sourceRootPath = "" # 存放TotalMode的根目录
+resultPath = "" # 保存sync分析结果
+streamProfileDict = dict() # 保存Python结果
+
+class PictureInfo(object):
+    class Struct(object):
+        def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+            self.deviceId = deviceId
+            self.sensorType = sensorType
+            self.syncTimeStamp = syncTimeStamp
+            self.picturePath = picturePath
+            self.deviceIdFull = deviceIdFull
+            self.fileExt = fileExt
+
+    def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+        return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt)
+
+class StreamProfileInfo:
+    def __init__(self, sensorType, width, height, format, fps):
+        self.sensorType = sensorType
+        self.width = width
+        self.height = height
+        self.format = format
+        self.fps = fps
+
+def initPrimaryId():
+    global primaryId
+
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Initialize primaryId failed. {deviceInfoPath} not exists")
+        return False
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Initialize primaryId failed.  {deviceInfoPath} not contain item 'devices'")
+        return False
+
+    for item in data['devices']:
+        if 'isPrimaryDevice'  in item and 'index' in item and item['isPrimaryDevice']:
+            primaryId = str(item['index'])
+            return True
+
+    return False
+
+def initSyncConfigParams():
+    global frameRate, tspRangeThreshold
+
+    configPath = f"{mainPythonWorkPath}/Config.ini"
+    if not os.path.exists(configPath):
+        print(f"Initliaze synchronized config parameter failed. {configPath} not exists.")
+        return False
+
+    # 创建ConfigParser对象
+    config = configparser.ConfigParser()
+
+    # 读取ini文件
+    config.read(configPath, 'utf-8')
+
+    if not config.has_option('Parameter', 'frameRate'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}")
+        return False
+
+    if not config.has_option('Parameter', 'tspRangeThreshold'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}")
+        return False
+
+    frameRate = config.getfloat('Parameter', 'frameRate')
+    if frameRate <= 0 or frameRate >= 1000:
+        print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}")
+        return False
+
+    tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold')
+    if tspRangeThreshold <= 0:
+        print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}")
+        return False
+
+    return True
+
+def getDeviceCount():
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Get device count failed. {deviceInfoPath} not exists")
+        return 0 
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Get device count failed.  {deviceInfoPath} not contain item 'devices'")
+        return 0
+
+    return len(data['devices'])
+
+def initProfileInfoDict():
+    global streamProfileDict
+
+    streamProfileDict = dict()
+    profilePath = f"{sourceRootPath}/StreamProfileInfo.txt"
+    if not os.path.exists(profilePath):
+        print(f"Initialize profile information dictionary failed. {profilePath} not exists")
+        return False
+
+    with open(profilePath, 'r') as f:
+        data = json.load(f)
+
+    if not 'streamProfiles' in data:
+        print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'")
+        return False
+
+    for item in data['streamProfiles']:
+        if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item:
+            profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps'])
+            streamProfileDict.setdefault(item['sensorType'], profileInfo)
+        else:
+            print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}")
+
+    return len(streamProfileDict.items()) > 0
+
+def initResultDir():
+    global resultPath
+    timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S")
+    resultPath = f"{sourceRootPath}/results-{timeText}"
+    if not os.path.exists(resultPath):
+        os.makedirs(resultPath)
+
+def isFrameFile(fileName):
+    fileExt = os.path.splitext(fileName)
+    if len(fileExt) < 2:
+        return False
+
+    frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"}
+    return fileExt[1] in frameExts
+
+def initPictureInfoDictionary(rootFilePath, pictureInfoDict):
+    frameFileCount = 0
+    for dirpath, dirnames, filenames in os.walk(rootFilePath):
+        # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath))
+        for fileName in filenames:
+            filePath = os.path.join(dirpath, fileName)
+            if not isFrameFile(fileName):
+                continue
+
+            frameFileCount += 1
+            fileNameNoExt = os.path.splitext(fileName)[0]
+            pictureNameList = fileNameNoExt.split('_')
+            deviceId = pictureNameList[2][5:]
+            sensorType = pictureNameList[0].replace('#', '_')
+            syncTimeStamp = int(pictureNameList[3][1:])
+
+            # 创建结构体
+            pictureInfo = PictureInfo()
+            info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1])
+
+            # 格式化字典一个key对应一个数组
+            pictureInfoDict.setdefault(deviceId, []).append(info)
+    
+    if 0 == frameFileCount:
+        print("initialize pictureInfoDict failed. Not found frame file")
+
+def matchFrame(pictureInfoDict):
+    global primaryId, frameRate
+    global streamProfileDict
+
+    # 获取比较图像数组,取第一个设备的Color数组来进行比较
+    compareList = []
+    for key in pictureInfoDict:
+        listTmp = list(pictureInfoDict[key])
+        for info in listTmp:
+            if info.deviceId == primaryId and info.sensorType == 'color':
+                compareList.append(info)
+
+    sorted_dict = dict(sorted(pictureInfoDict.items(), key=lambda x: x[1], reverse=False))
+    pictureInfoDict = sorted_dict
+
+    # 根据开流情况动态分析，减少循环次数
+    hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict
+    hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict
+    hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict
+
+    # 匹配相邻帧
+    resultDict = {}
+    consumedList = []
+    dictIndex = 0
+    frameTspGap = int(1000.0/frameRate+0.5)
+    for comparePic in compareList:
+        resultDict.setdefault(dictIndex, []).append(comparePic)
+        for key in pictureInfoDict:
+            listTmp = list(pictureInfoDict[key])
+            # 对比彩色
+            if hasColorProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'color' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对Depth
+            if hasDepthProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'depth' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # IR
+            if hasIRProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+        # 计算偏差
+        for info in resultDict[dictIndex]:
+            info.tspDiff = info.syncTimeStamp - comparePic.syncTimeStamp
+
+        dictIndex += 1
+
+    return resultDict
+
+
+def safe_make_dir(path):
+    if not os.path.exists(path):
+        os.makedirs(path)
+
+
+def handleSyncFrames():
+    global resultPath
+    global sourceFramesPath
+    global streamProfileDict
+
+    fp = open(f"{resultPath}/frameMatchLog.txt", "w")
+
+    if not initProfileInfoDict():
+        print("Initialize stream profile dictionary failed. exit")
+        return
+    print(f"Stream profiles: {streamProfileDict}")
+    streamProfileCount = len(streamProfileDict.items())
+
+    pictureInfoDict = {}
+
+    initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict)
+    if len(pictureInfoDict) <= 0:
+        print("init pictureInfoDict failed. pictureInfoDict.len = 0")
+        return
+
+    # # Dump pictureInfoDict
+    # for key in pictureInfoDict:
+    #     print("===================key:%s" % key)
+    #     listTmp = list(pictureInfoDict[key])
+    # for info in listTmp:
+    #     print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath))
+
+    deviceCount = getDeviceCount()
+    print("=================device count:%d" % deviceCount)
+
+    resultDict = {}
+    resultDict = matchFrame(pictureInfoDict)
+
+    for key in resultDict:
+        fp.write("===================result key:%s\n" % key)
+        print("===================result key:%s" % key)
+        listTmp = list(resultDict[key])
+        for info in listTmp:
+            fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+            print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+
+    matchPath = f"{resultPath}/matchFrames/"
+    safe_make_dir(matchPath)
+    notMatchPath = f"{resultPath}/notMatchFrames/"
+    abnormalPath = f"{resultPath}/abnormal/"
+
+    for key in resultDict:
+        listTmp = list(resultDict[key])
+        if (len(listTmp) == (deviceCount * streamProfileCount)):
+            haveAbnormalData = False
+            for info in listTmp:
+                if info.tspDiff >= tspRangeThreshold:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+                    haveAbnormalData = True
+                else:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+
+            if haveAbnormalData:
+                safe_make_dir(abnormalPath)
+                for info in listTmp:
+                    if info.tspDiff >= tspRangeThreshold:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt 
+                        shutil.copy(info.picturePath, path)
+                    else:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                        shutil.copy(info.picturePath, path)
+        else:
+            safe_make_dir(notMatchPath)
+            for info in listTmp:
+                newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                    info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                shutil.copy(info.picturePath, newFilePath)
+
+def main():
+    if not initPrimaryId():
+        print("Initialize primaryId failed. exit.")
+        return
+
+    if not initSyncConfigParams():
+        print("Initialized Sync config parameter failed. exit.")
+        return
+
+    initResultDir()
+
+    print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}")
+    handleSyncFrames()
+
+    return
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Astra2'")
+    parser.add_argument('frames_dir', type=str, help="Frames directory")
+    parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory")
+    args = parser.parse_args()
+
+    sourceRootPath = args.frames_dir
+    sourceFramesPath = f"{args.frames_dir}/TotalModeFrames"
+    mainPythonWorkPath = args.main_script_root_dir
+    print(f"Start of Astra2 synchronize frame. sourceRootPath={sourceRootPath}")
+
+    main()
+
+    print("Finish of Astra2 synchronize frame.")
+
+
+
+
+
+

orbbec_camera/config/multicamera_sync/Python/script/config_frameMatch-systemTimestamp.py

@@ -0,0 +1,408 @@
+import os.path
+import argparse
+import sys
+import json
+import shutil
+import configparser
+
+from pathlib import Path
+from datetime import datetime
+
+## Config.ini中配置
+# 单位：FPS,请依据数据集实际帧率填写，否则会导致匹配不准确
+frameRate = -1
+# 单位：ms，当匹配后某组数据帧的时间戳极差大于等于tspRangeThreshold，文件名会增加标注
+tspRangeThreshold = -1
+
+## Python脚本自动解析, 不要修改
+primaryId = '-1' #主机ID
+mainPythonWorkPath = "" # SyncFramesMain.py的工作目录
+sourceFramesPath = ""  # 请勿修改，固定值,TotalMode目录路径
+sourceRootPath = "" # 存放TotalMode的根目录
+resultPath = "" # 保存sync分析结果
+streamProfileDict = dict() # 保存Python结果
+
+class PictureInfo(object):
+    class Struct(object):
+        def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+            self.deviceId = deviceId
+            self.sensorType = sensorType
+            self.syncTimeStamp = syncTimeStamp
+            self.picturePath = picturePath
+            self.deviceIdFull = deviceIdFull
+            self.fileExt = fileExt
+
+    def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+        return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt)
+
+class StreamProfileInfo:
+    def __init__(self, sensorType, width, height, format, fps):
+        self.sensorType = sensorType
+        self.width = width
+        self.height = height
+        self.format = format
+        self.fps = fps
+
+def initPrimaryId():
+    global primaryId
+
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Initialize primaryId failed. {deviceInfoPath} not exists")
+        return False
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Initialize primaryId failed.  {deviceInfoPath} not contain item 'devices'")
+        return False
+
+    for item in data['devices']:
+        if 'isPrimaryDevice'  in item and 'index' in item and item['isPrimaryDevice']:
+            primaryId = str(item['index'])
+            return True
+
+    return False
+
+def initSyncConfigParams():
+    global frameRate, tspRangeThreshold
+
+    configPath = f"{mainPythonWorkPath}/Config.ini"
+    if not os.path.exists(configPath):
+        print(f"Initliaze synchronized config parameter failed. {configPath} not exists.")
+        return False
+
+    # 创建ConfigParser对象
+    config = configparser.ConfigParser()
+
+    # 读取ini文件
+    config.read(configPath, 'utf-8')
+
+    if not config.has_option('Parameter', 'frameRate'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}")
+        return False
+
+    if not config.has_option('Parameter', 'tspRangeThreshold'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}")
+        return False
+
+    frameRate = config.getfloat('Parameter', 'frameRate')
+    if frameRate <= 0 or frameRate >= 1000:
+        print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}")
+        return False
+
+    tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold')
+    if tspRangeThreshold <= 0:
+        print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}")
+        return False
+
+    return True
+
+def getDeviceCount():
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Get device count failed. {deviceInfoPath} not exists")
+        return 0 
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Get device count failed.  {deviceInfoPath} not contain item 'devices'")
+        return 0
+
+    return len(data['devices'])
+
+def initProfileInfoDict():
+    global streamProfileDict
+
+    streamProfileDict = dict()
+    profilePath = f"{sourceRootPath}/StreamProfileInfo.txt"
+    if not os.path.exists(profilePath):
+        print(f"Initialize profile information dictionary failed. {profilePath} not exists")
+        return False
+
+    with open(profilePath, 'r') as f:
+        data = json.load(f)
+
+    if not 'streamProfiles' in data:
+        print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'")
+        return False
+
+    for item in data['streamProfiles']:
+        if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item:
+            profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps'])
+            streamProfileDict.setdefault(item['sensorType'], profileInfo)
+        else:
+            print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}")
+
+    return len(streamProfileDict.items()) > 0
+
+def initResultDir():
+    global resultPath
+    timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S")
+    resultPath = f"{sourceRootPath}/results-{timeText}"
+    if not os.path.exists(resultPath):
+        os.makedirs(resultPath)
+
+def isFrameFile(fileName):
+    fileExt = os.path.splitext(fileName)
+    if len(fileExt) < 2:
+        return False
+
+    frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"}
+    return fileExt[1] in frameExts
+
+def initPictureInfoDictionary(rootFilePath, pictureInfoDict):
+    frameFileCount = 0
+    for dirpath, dirnames, filenames in os.walk(rootFilePath):
+        # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath))
+        for fileName in filenames:
+            filePath = os.path.join(dirpath, fileName)
+            if not isFrameFile(fileName):
+                continue
+
+            frameFileCount += 1
+            fileNameNoExt = os.path.splitext(fileName)[0]
+            pictureNameList = fileNameNoExt.split('_')
+            deviceId = pictureNameList[2][5:]
+            sensorType = pictureNameList[0].replace('#', '_')
+            syncTimeStamp = int(pictureNameList[5][1:])
+
+            # 创建结构体
+            pictureInfo = PictureInfo()
+            info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1])
+
+            # 格式化字典一个key对应一个数组
+            pictureInfoDict.setdefault(deviceId, []).append(info)
+    
+    if 0 == frameFileCount:
+        print("initialize pictureInfoDict failed. Not found frame file")
+
+
+def matchFrame(pictureInfoDict):
+    global primaryId, frameRate
+    global streamProfileDict
+
+    # 获取比较图像数组,取第一个设备的Color数组来进行比较
+    compareList = []
+    for key in pictureInfoDict:
+        listTmp = list(pictureInfoDict[key])
+        for info in listTmp:
+            if info.deviceId == primaryId and info.sensorType == 'color':
+                compareList.append(info)
+
+    # 根据开流情况动态分析，减少循环次数
+    hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict
+    hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict
+    hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict
+    hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict
+    hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict
+
+    # 匹配相邻帧
+    resultDict = {}
+    consumedList = []
+    dictIndex = 0
+    frameTspHalfGap = int(1000.0/frameRate/2.0+0.5)
+    for comparePic in compareList:
+        resultDict.setdefault(dictIndex, []).append(comparePic)
+        for key in pictureInfoDict:
+            listTmp = list(pictureInfoDict[key])
+            # 对比彩色
+            if hasColorProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对Depth
+            if hasDepthProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # IR
+            if hasIRProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 左IR
+            if hasIRLeftProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 右IR
+            if hasIRRightProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+        # 计算偏差
+        minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp)
+        for info in resultDict[dictIndex]:
+            info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp
+
+        dictIndex += 1
+
+    return resultDict
+
+
+def safe_make_dir(path):
+    if not os.path.exists(path):
+        os.makedirs(path)
+
+
+def handleSyncFrames():
+    global resultPath
+    global sourceFramesPath
+    global streamProfileDict
+
+    fp = open(f"{resultPath}/frameMatchLog.txt", "w")
+
+    if not initProfileInfoDict():
+        print("Initialize stream profile dictionary failed. exit")
+        return
+    print(f"Stream profiles: {streamProfileDict}")
+    streamProfileCount = len(streamProfileDict.items())
+
+    pictureInfoDict = {}
+
+    initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict)
+    if len(pictureInfoDict) <= 0:
+        print("init pictureInfoDict failed. pictureInfoDict.len = 0")
+        return
+
+    # # Dump pictureInfoDict
+    # for key in pictureInfoDict:
+    #     print("===================key:%s" % key)
+    #     listTmp = list(pictureInfoDict[key])
+    # for info in listTmp:
+    #     print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath))
+
+    deviceCount = getDeviceCount()
+    print("=================device count:%d" % deviceCount)
+
+    resultDict = {}
+    resultDict = matchFrame(pictureInfoDict)
+
+    for key in resultDict:
+        fp.write("===================result key:%s\n" % key)
+        print("===================result key:%s" % key)
+        listTmp = list(resultDict[key])
+        for info in listTmp:
+            fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+            print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+
+    matchPath = f"{resultPath}/matchFrames/"
+    safe_make_dir(matchPath)
+    notMatchPath = f"{resultPath}/notMatchFrames/"
+    abnormalPath = f"{resultPath}/abnormal/"
+
+    for key in resultDict:
+        listTmp = list(resultDict[key])
+        if (len(listTmp) == (deviceCount * streamProfileCount)):
+            haveAbnormalData = False
+            for info in listTmp:
+                if info.tspDiff >= tspRangeThreshold:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+                    haveAbnormalData = True
+                else:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+
+            if haveAbnormalData:
+                safe_make_dir(abnormalPath)
+                for info in listTmp:
+                    if info.tspDiff >= tspRangeThreshold:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt 
+                        shutil.copy(info.picturePath, path)
+                    else:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                        shutil.copy(info.picturePath, path)
+        else:
+            safe_make_dir(notMatchPath)
+            for info in listTmp:
+                newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                    info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                shutil.copy(info.picturePath, newFilePath)
+
+def main():
+    if not initPrimaryId():
+        print("Initialize primaryId failed. exit.")
+        return
+
+    if not initSyncConfigParams():
+        print("Initialized Sync config parameter failed. exit.")
+        return
+
+    initResultDir()
+
+    print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}")
+    handleSyncFrames()
+
+    return
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Gemini 2'")
+    parser.add_argument('frames_dir', type=str, help="Frames directory")
+    parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory")
+    args = parser.parse_args()
+
+    sourceRootPath = args.frames_dir
+    sourceFramesPath = f"{args.frames_dir}/TotalModeFrames"
+    mainPythonWorkPath = args.main_script_root_dir
+    print(f"Start of Gemini2 synchronize frame. sourceRootPath={sourceRootPath}")
+
+    main()
+
+    print("Finish of Gemini2 synchronize frame.")

orbbec_camera/config/multicamera_sync/Python/script/config_frameMatch.py

@@ -0,0 +1,423 @@
+import os.path
+import argparse
+import sys
+import json
+import shutil
+import configparser
+
+from pathlib import Path
+from datetime import datetime
+
+import os
+import re
+
+## Config.ini中配置
+# 单位：FPS,请依据数据集实际帧率填写，否则会导致匹配不准确
+frameRate = -1
+# 单位：ms，当匹配后某组数据帧的时间戳极差大于等于tspRangeThreshold，文件名会增加标注
+tspRangeThreshold = -1
+
+## Python脚本自动解析, 不要修改
+primaryId = '-1' #主机ID
+mainPythonWorkPath = "" # SyncFramesMain.py的工作目录
+sourceFramesPath = ""  # 请勿修改，固定值,TotalMode目录路径
+sourceRootPath = "" # 存放TotalMode的根目录
+resultPath = "" # 保存sync分析结果
+streamProfileDict = dict() # 保存Python结果
+
+class PictureInfo(object):
+    class Struct(object):
+        def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+            self.deviceId = deviceId
+            self.sensorType = sensorType
+            self.syncTimeStamp = syncTimeStamp
+            self.picturePath = picturePath
+            self.deviceIdFull = deviceIdFull
+            self.fileExt = fileExt
+
+    def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+        return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt)
+
+class StreamProfileInfo:
+    def __init__(self, sensorType, width, height, format, fps):
+        self.sensorType = sensorType
+        self.width = width
+        self.height = height
+        self.format = format
+        self.fps = fps
+
+def initPrimaryId():
+    global primaryId
+
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Initialize primaryId failed. {deviceInfoPath} not exists")
+        return False
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Initialize primaryId failed.  {deviceInfoPath} not contain item 'devices'")
+        return False
+
+    for item in data['devices']:
+        if 'isPrimaryDevice'  in item and 'index' in item and item['isPrimaryDevice']:
+            primaryId = str(item['index'])
+            return True
+
+    return False
+
+def initSyncConfigParams():
+    global frameRate, tspRangeThreshold
+
+    configPath = f"{mainPythonWorkPath}/Config.ini"
+    if not os.path.exists(configPath):
+        print(f"Initliaze synchronized config parameter failed. {configPath} not exists.")
+        return False
+
+    # 创建ConfigParser对象
+    config = configparser.ConfigParser()
+
+    # 读取ini文件
+    config.read(configPath, 'utf-8')
+
+    if not config.has_option('Parameter', 'frameRate'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}")
+        return False
+
+    if not config.has_option('Parameter', 'tspRangeThreshold'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}")
+        return False
+
+    frameRate = config.getfloat('Parameter', 'frameRate')
+    if frameRate <= 0 or frameRate >= 1000:
+        print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}")
+        return False
+
+    tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold')
+    if tspRangeThreshold <= 0:
+        print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}")
+        return False
+
+    return True
+
+def getDeviceCount():
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Get device count failed. {deviceInfoPath} not exists")
+        return 0 
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Get device count failed.  {deviceInfoPath} not contain item 'devices'")
+        return 0
+
+    return len(data['devices'])
+
+def initProfileInfoDict():
+    global streamProfileDict
+
+    streamProfileDict = dict()
+    profilePath = f"{sourceRootPath}/StreamProfileInfo.txt"
+    if not os.path.exists(profilePath):
+        print(f"Initialize profile information dictionary failed. {profilePath} not exists")
+        return False
+
+    with open(profilePath, 'r') as f:
+        data = json.load(f)
+
+    if not 'streamProfiles' in data:
+        print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'")
+        return False
+
+    for item in data['streamProfiles']:
+        if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item:
+            profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps'])
+            streamProfileDict.setdefault(item['sensorType'], profileInfo)
+        else:
+            print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}")
+
+    return len(streamProfileDict.items()) > 0
+
+def initResultDir():
+    global resultPath
+    timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S")
+    resultPath = f"{sourceRootPath}/results-{timeText}"
+    if not os.path.exists(resultPath):
+        os.makedirs(resultPath)
+
+def isFrameFile(fileName):
+    fileExt = os.path.splitext(fileName)
+    if len(fileExt) < 2:
+        return False
+
+    frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"}
+    return fileExt[1] in frameExts
+
+def extract_number(filename): 
+    """提取文件名中以'g'开头的数字部分"""
+    match_g = re.search(r'g(\d+)', filename)
+    if match_g:
+        return int(match_g.group(1))
+    
+    """提取文件名中以'd'开头的数字部分"""
+    match_d = re.search(r'd(\d+)', filename)
+    if match_d:
+        return int(match_d.group(1))
+    return float('inf')
+
+def initPictureInfoDictionary(rootFilePath, pictureInfoDict):
+    frameFileCount = 0
+    for dirpath, dirnames, filenames in os.walk(rootFilePath):
+        filenames.sort(key=extract_number) # 排序，按文件名中的'd'和'g'后面的数字从小到大排序
+        for fileName in filenames:
+            filePath = os.path.join(dirpath, fileName)
+            if not isFrameFile(fileName):
+                continue
+
+            frameFileCount += 1
+            fileNameNoExt = os.path.splitext(fileName)[0]
+            pictureNameList = fileNameNoExt.split('_')
+            deviceId = pictureNameList[2][5:]
+            sensorType = pictureNameList[0].replace('#', '_')
+            syncTimeStamp = int(pictureNameList[3][1:])
+
+            # 创建结构体
+            pictureInfo = PictureInfo()
+            info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1])
+
+            # 格式化字典一个key对应一个数组
+            pictureInfoDict.setdefault(deviceId, []).append(info)
+    
+    if 0 == frameFileCount:
+        print("initialize pictureInfoDict failed. Not found frame file")
+
+
+def matchFrame(pictureInfoDict):
+    global primaryId, frameRate
+    global streamProfileDict
+
+    # 获取比较图像数组,取第一个设备的Color数组来进行比较
+    compareList = []
+    for key in pictureInfoDict:
+        listTmp = list(pictureInfoDict[key])
+        for info in listTmp:
+            if info.deviceId == primaryId and info.sensorType == 'color':
+                compareList.append(info)
+
+    # 根据开流情况动态分析，减少循环次数
+    hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict
+    hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict
+    hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict
+    hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict
+    hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict
+
+    # 匹配相邻帧
+    resultDict = {}
+    consumedList = []
+    dictIndex = 0
+    frameTspHalfGap = int(1000.0/frameRate/2.0+0.5)
+    for comparePic in compareList:
+        resultDict.setdefault(dictIndex, []).append(comparePic)
+        for key in pictureInfoDict:
+            listTmp = list(pictureInfoDict[key])
+            # 对比彩色
+            if hasColorProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对Depth
+            if hasDepthProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # IR
+            if hasIRProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对左IR
+            if hasIRLeftProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对右IR
+            if hasIRRightProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+        # 计算偏差
+        minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp)
+        for info in resultDict[dictIndex]:
+            info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp
+
+        dictIndex += 1
+
+    return resultDict
+
+
+def safe_make_dir(path):
+    if not os.path.exists(path):
+        os.makedirs(path)
+
+
+def handleSyncFrames():
+    global resultPath
+    global sourceFramesPath
+    global streamProfileDict
+
+    fp = open(f"{resultPath}/frameMatchLog.txt", "w")
+
+    if not initProfileInfoDict():
+        print("Initialize stream profile dictionary failed. exit")
+        return
+    print(f"Stream profiles: {streamProfileDict}")
+    streamProfileCount = len(streamProfileDict.items())
+
+    pictureInfoDict = {}
+
+    initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict)
+    if len(pictureInfoDict) <= 0:
+        print("init pictureInfoDict failed. pictureInfoDict.len = 0")
+        return
+
+    # # Dump pictureInfoDict
+    # for key in pictureInfoDict:
+    #     print("===================key:%s" % key)
+    #     listTmp = list(pictureInfoDict[key])
+    # for info in listTmp:
+    #     print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath))
+
+    deviceCount = getDeviceCount()
+    print("=================device count:%d" % deviceCount)
+
+    resultDict = {}
+    resultDict = matchFrame(pictureInfoDict)
+
+    for key in resultDict:
+        fp.write("===================result key:%s\n" % key)
+        print("===================result key:%s" % key)
+        listTmp = list(resultDict[key])
+        for info in listTmp:
+            fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+            print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+
+    matchPath = f"{resultPath}/matchFrames/"
+    safe_make_dir(matchPath)
+    notMatchPath = f"{resultPath}/notMatchFrames/"
+    abnormalPath = f"{resultPath}/abnormal/"
+
+    for key in resultDict:
+        listTmp = list(resultDict[key])
+        if (len(listTmp) == (deviceCount * streamProfileCount)):
+            haveAbnormalData = False
+            for info in listTmp:
+                if info.tspDiff >= tspRangeThreshold:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+                    haveAbnormalData = True
+                else:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+
+            if haveAbnormalData:
+                safe_make_dir(abnormalPath)
+                for info in listTmp:
+                    if info.tspDiff >= tspRangeThreshold:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt 
+                        shutil.copy(info.picturePath, path)
+                    else:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                        shutil.copy(info.picturePath, path)
+        else:
+            safe_make_dir(notMatchPath)
+            for info in listTmp:
+                newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                    info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                shutil.copy(info.picturePath, newFilePath)
+
+def main():
+    if not initPrimaryId():
+        print("Initialize primaryId failed. exit.")
+        return
+
+    if not initSyncConfigParams():
+        print("Initialized Sync config parameter failed. exit.")
+        return
+
+    initResultDir()
+
+    print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}")
+    handleSyncFrames()
+
+    return
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Devices'")
+    parser.add_argument('frames_dir', type=str, help="Frames directory")
+    parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory")
+    args = parser.parse_args()
+
+    sourceRootPath = args.frames_dir
+    sourceFramesPath = f"{args.frames_dir}/TotalModeFrames"
+    mainPythonWorkPath = args.main_script_root_dir
+    print(f"Start of CommonFrameMatch synchronize frame. sourceRootPath={sourceRootPath}")
+
+    main()
+
+    print("Finish of CommonFrameMatch synchronize frame.")

orbbec_camera/config/multicamera_sync/Python/script/config_frameMatch_Gemini2VL.py

@@ -0,0 +1,408 @@
+import os.path
+import argparse
+import sys
+import json
+import shutil
+import configparser
+
+from pathlib import Path
+from datetime import datetime
+
+## Config.ini中配置
+# 单位：FPS,请依据数据集实际帧率填写，否则会导致匹配不准确
+frameRate = -1
+# 单位：ms，当匹配后某组数据帧的时间戳极差大于等于tspRangeThreshold，文件名会增加标注
+tspRangeThreshold = -1
+
+## Python脚本自动解析, 不要修改
+primaryId = '-1' #主机ID
+mainPythonWorkPath = "" # SyncFramesMain.py的工作目录
+sourceFramesPath = ""  # 请勿修改，固定值,TotalMode目录路径
+sourceRootPath = "" # 存放TotalMode的根目录
+resultPath = "" # 保存sync分析结果
+streamProfileDict = dict() # 保存Python结果
+
+class PictureInfo(object):
+    class Struct(object):
+        def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+            self.deviceId = deviceId
+            self.sensorType = sensorType
+            self.syncTimeStamp = syncTimeStamp
+            self.picturePath = picturePath
+            self.deviceIdFull = deviceIdFull
+            self.fileExt = fileExt
+
+    def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt):
+        return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt)
+
+class StreamProfileInfo:
+    def __init__(self, sensorType, width, height, format, fps):
+        self.sensorType = sensorType
+        self.width = width
+        self.height = height
+        self.format = format
+        self.fps = fps
+
+def initPrimaryId():
+    global primaryId
+
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Initialize primaryId failed. {deviceInfoPath} not exists")
+        return False
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Initialize primaryId failed.  {deviceInfoPath} not contain item 'devices'")
+        return False
+
+    for item in data['devices']:
+        if 'isPrimaryDevice'  in item and 'index' in item and item['isPrimaryDevice']:
+            primaryId = str(item['index'])
+            return True
+
+    return False
+
+def initSyncConfigParams():
+    global frameRate, tspRangeThreshold
+
+    configPath = f"{mainPythonWorkPath}/Config.ini"
+    if not os.path.exists(configPath):
+        print(f"Initliaze synchronized config parameter failed. {configPath} not exists.")
+        return False
+
+    # 创建ConfigParser对象
+    config = configparser.ConfigParser()
+
+    # 读取ini文件
+    config.read(configPath, 'utf-8')
+
+    if not config.has_option('Parameter', 'frameRate'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}")
+        return False
+
+    if not config.has_option('Parameter', 'tspRangeThreshold'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}")
+        return False
+
+    frameRate = config.getfloat('Parameter', 'frameRate')
+    if frameRate <= 0 or frameRate >= 1000:
+        print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}")
+        return False
+
+    tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold')
+    if tspRangeThreshold <= 0:
+        print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}")
+        return False
+
+    return True
+
+def getDeviceCount():
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Get device count failed. {deviceInfoPath} not exists")
+        return 0 
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Get device count failed.  {deviceInfoPath} not contain item 'devices'")
+        return 0
+
+    return len(data['devices'])
+
+def initProfileInfoDict():
+    global streamProfileDict
+
+    streamProfileDict = dict()
+    profilePath = f"{sourceRootPath}/StreamProfileInfo.txt"
+    if not os.path.exists(profilePath):
+        print(f"Initialize profile information dictionary failed. {profilePath} not exists")
+        return False
+
+    with open(profilePath, 'r') as f:
+        data = json.load(f)
+
+    if not 'streamProfiles' in data:
+        print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'")
+        return False
+
+    for item in data['streamProfiles']:
+        if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item:
+            profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps'])
+            streamProfileDict.setdefault(item['sensorType'], profileInfo)
+        else:
+            print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}")
+
+    return len(streamProfileDict.items()) > 0
+
+def initResultDir():
+    global resultPath
+    timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S")
+    resultPath = f"{sourceRootPath}/results-{timeText}"
+    if not os.path.exists(resultPath):
+        os.makedirs(resultPath)
+
+def isFrameFile(fileName):
+    fileExt = os.path.splitext(fileName)
+    if len(fileExt) < 2:
+        return False
+
+    frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"}
+    return fileExt[1] in frameExts
+
+def initPictureInfoDictionary(rootFilePath, pictureInfoDict):
+    frameFileCount = 0
+    for dirpath, dirnames, filenames in os.walk(rootFilePath):
+        # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath))
+        for fileName in filenames:
+            filePath = os.path.join(dirpath, fileName)
+            if not isFrameFile(fileName):
+                continue
+
+            frameFileCount += 1
+            fileNameNoExt = os.path.splitext(fileName)[0]
+            pictureNameList = fileNameNoExt.split('_')
+            deviceId = pictureNameList[2][5:]
+            sensorType = pictureNameList[0].replace('#', '_')
+            syncTimeStamp = int(pictureNameList[3][1:])
+
+            # 创建结构体
+            pictureInfo = PictureInfo()
+            info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1])
+
+            # 格式化字典一个key对应一个数组
+            pictureInfoDict.setdefault(deviceId, []).append(info)
+    
+    if 0 == frameFileCount:
+        print("initialize pictureInfoDict failed. Not found frame file")
+
+
+def matchFrame(pictureInfoDict):
+    global primaryId, frameRate
+    global streamProfileDict
+
+    # 获取比较图像数组,取第一个设备的Color数组来进行比较
+    compareList = []
+    for key in pictureInfoDict:
+        listTmp = list(pictureInfoDict[key])
+        for info in listTmp:
+            if info.deviceId == primaryId and info.sensorType == 'color':
+                compareList.append(info)
+
+    # 根据开流情况动态分析，减少循环次数
+    hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict
+    hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict
+    hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict
+    hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict
+    hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict
+
+    # 匹配相邻帧
+    resultDict = {}
+    consumedList = []
+    dictIndex = 0
+    frameTspHalfGap = int(1000.0/frameRate/2.0+0.5)
+    for comparePic in compareList:
+        resultDict.setdefault(dictIndex, []).append(comparePic)
+        for key in pictureInfoDict:
+            listTmp = list(pictureInfoDict[key])
+            # 对比彩色
+            if hasColorProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对Depth
+            if hasDepthProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # IR
+            if hasIRProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对左IR
+            if hasIRLeftProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对右IR
+            if hasIRRightProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+        # 计算偏差
+        minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp)
+        for info in resultDict[dictIndex]:
+            info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp
+
+        dictIndex += 1
+
+    return resultDict
+
+
+def safe_make_dir(path):
+    if not os.path.exists(path):
+        os.makedirs(path)
+
+
+def handleSyncFrames():
+    global resultPath
+    global sourceFramesPath
+    global streamProfileDict
+
+    fp = open(f"{resultPath}/frameMatchLog.txt", "w")
+
+    if not initProfileInfoDict():
+        print("Initialize stream profile dictionary failed. exit")
+        return
+    print(f"Stream profiles: {streamProfileDict}")
+    streamProfileCount = len(streamProfileDict.items())
+
+    pictureInfoDict = {}
+
+    initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict)
+    if len(pictureInfoDict) <= 0:
+        print("init pictureInfoDict failed. pictureInfoDict.len = 0")
+        return
+
+    # # Dump pictureInfoDict
+    # for key in pictureInfoDict:
+    #     print("===================key:%s" % key)
+    #     listTmp = list(pictureInfoDict[key])
+    # for info in listTmp:
+    #     print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath))
+
+    deviceCount = getDeviceCount()
+    print("=================device count:%d" % deviceCount)
+
+    resultDict = {}
+    resultDict = matchFrame(pictureInfoDict)
+
+    for key in resultDict:
+        fp.write("===================result key:%s\n" % key)
+        print("===================result key:%s" % key)
+        listTmp = list(resultDict[key])
+        for info in listTmp:
+            fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+            print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+
+    matchPath = f"{resultPath}/matchFrames/"
+    safe_make_dir(matchPath)
+    notMatchPath = f"{resultPath}/notMatchFrames/"
+    abnormalPath = f"{resultPath}/abnormal/"
+
+    for key in resultDict:
+        listTmp = list(resultDict[key])
+        if (len(listTmp) == (deviceCount * streamProfileCount)):
+            haveAbnormalData = False
+            for info in listTmp:
+                if info.tspDiff >= tspRangeThreshold:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+                    haveAbnormalData = True
+                else:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                    shutil.copy(info.picturePath, path)
+
+            if haveAbnormalData:
+                safe_make_dir(abnormalPath)
+                for info in listTmp:
+                    if info.tspDiff >= tspRangeThreshold:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt 
+                        shutil.copy(info.picturePath, path)
+                    else:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                        shutil.copy(info.picturePath, path)
+        else:
+            safe_make_dir(notMatchPath)
+            for info in listTmp:
+                newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                    info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt
+                shutil.copy(info.picturePath, newFilePath)
+
+def main():
+    if not initPrimaryId():
+        print("Initialize primaryId failed. exit.")
+        return
+
+    if not initSyncConfigParams():
+        print("Initialized Sync config parameter failed. exit.")
+        return
+
+    initResultDir()
+
+    print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}")
+    handleSyncFrames()
+
+    return
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Gemini 2 VL'")
+    parser.add_argument('frames_dir', type=str, help="Frames directory")
+    parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory")
+    args = parser.parse_args()
+
+    sourceRootPath = args.frames_dir
+    sourceFramesPath = f"{args.frames_dir}/TotalModeFrames"
+    mainPythonWorkPath = args.main_script_root_dir
+    print(f"Start of Gemini2VL synchronize frame. sourceRootPath={sourceRootPath}")
+
+    main()
+
+    print("Finish of Gemini2VL synchronize frame.")

orbbec_camera/config/multicamera_sync/Python/script/config_frameMatch_g330.py

@@ -0,0 +1,427 @@
+import os.path
+import argparse
+import sys
+import json
+import shutil
+import configparser
+
+from pathlib import Path
+from datetime import datetime
+
+import os
+import re
+
+## Config.ini中配置
+# 单位：FPS,请依据数据集实际帧率填写，否则会导致匹配不准确
+frameRate = -1
+# 单位：ms，当匹配后某组数据帧的时间戳极差大于等于tspRangeThreshold，文件名会增加标注
+tspRangeThreshold = -1
+
+## Python脚本自动解析, 不要修改
+primaryId = '-1' #主机ID
+mainPythonWorkPath = "" # SyncFramesMain.py的工作目录
+sourceFramesPath = ""  # 请勿修改，固定值,TotalMode目录路径
+sourceRootPath = "" # 存放TotalMode的根目录
+resultPath = "" # 保存sync分析结果
+streamProfileDict = dict() # 保存Python结果
+
+class PictureInfo(object):
+    class Struct(object):
+        def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain):
+            self.deviceId = deviceId
+            self.sensorType = sensorType
+            self.syncTimeStamp = syncTimeStamp
+            self.picturePath = picturePath
+            self.deviceIdFull = deviceIdFull
+            self.fileExt = fileExt
+            self.exp = exp
+            self.gain = gain
+
+    def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain):
+        return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain)
+
+class StreamProfileInfo:
+    def __init__(self, sensorType, width, height, format, fps):
+        self.sensorType = sensorType
+        self.width = width
+        self.height = height
+        self.format = format
+        self.fps = fps
+
+def initPrimaryId():
+    global primaryId
+
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Initialize primaryId failed. {deviceInfoPath} not exists")
+        return False
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Initialize primaryId failed.  {deviceInfoPath} not contain item 'devices'")
+        return False
+
+    for item in data['devices']:
+        if 'isPrimaryDevice'  in item and 'index' in item and item['isPrimaryDevice']:
+            primaryId = str(item['index'])
+            return True
+
+    return False
+
+def initSyncConfigParams():
+    global frameRate, tspRangeThreshold
+
+    configPath = f"{mainPythonWorkPath}/Config.ini"
+    if not os.path.exists(configPath):
+        print(f"Initliaze synchronized config parameter failed. {configPath} not exists.")
+        return False
+
+    # 创建ConfigParser对象
+    config = configparser.ConfigParser()
+
+    # 读取ini文件
+    config.read(configPath, 'utf-8')
+
+    if not config.has_option('Parameter', 'frameRate'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}")
+        return False
+
+    if not config.has_option('Parameter', 'tspRangeThreshold'):
+        print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}")
+        return False
+
+    frameRate = config.getfloat('Parameter', 'frameRate')
+    if frameRate <= 0 or frameRate >= 1000:
+        print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}")
+        return False
+
+    tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold')
+    if tspRangeThreshold <= 0:
+        print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}")
+        return False
+
+    return True
+
+def getDeviceCount():
+    deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt"
+    if not os.path.exists(deviceInfoPath):
+        print(f"Get device count failed. {deviceInfoPath} not exists")
+        return 0 
+
+    with open(deviceInfoPath, 'r') as f:
+        data = json.load(f)
+
+    if not 'devices' in data:
+        print(f"Get device count failed.  {deviceInfoPath} not contain item 'devices'")
+        return 0
+
+    return len(data['devices'])
+
+def initProfileInfoDict():
+    global streamProfileDict
+
+    streamProfileDict = dict()
+    profilePath = f"{sourceRootPath}/StreamProfileInfo.txt"
+    if not os.path.exists(profilePath):
+        print(f"Initialize profile information dictionary failed. {profilePath} not exists")
+        return False
+
+    with open(profilePath, 'r') as f:
+        data = json.load(f)
+
+    if not 'streamProfiles' in data:
+        print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'")
+        return False
+
+    for item in data['streamProfiles']:
+        if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item:
+            profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps'])
+            streamProfileDict.setdefault(item['sensorType'], profileInfo)
+        else:
+            print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}")
+
+    return len(streamProfileDict.items()) > 0
+
+def initResultDir():
+    global resultPath
+    timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S")
+    resultPath = f"{sourceRootPath}/results-{timeText}"
+    if not os.path.exists(resultPath):
+        os.makedirs(resultPath)
+
+def isFrameFile(fileName):
+    fileExt = os.path.splitext(fileName)
+    if len(fileExt) < 2:
+        return False
+
+    frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"}
+    return fileExt[1] in frameExts
+
+def extract_number(filename): 
+    match_d = re.search(r'_d(\d+)', filename)
+    if match_d:
+        return int(match_d.group(1))
+
+    match_g = re.search(r'_g(\d+)', filename)
+    if match_g:
+        return int(match_g.group(1))
+    
+    return float('inf')
+
+def initPictureInfoDictionary(rootFilePath, pictureInfoDict):
+    frameFileCount = 0
+    for dirpath, dirnames, filenames in os.walk(rootFilePath):
+        filenames.sort(key=extract_number) # 排序，按文件名中的'd'和'g'后面的数字从小到大排序
+        # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath))
+        for fileName in filenames:
+            filePath = os.path.join(dirpath, fileName)
+            if not isFrameFile(fileName):
+                continue
+
+            frameFileCount += 1
+            fileNameNoExt = os.path.splitext(fileName)[0]
+            pictureNameList = fileNameNoExt.split('_')
+            deviceId = pictureNameList[2][5:]
+            sensorType = pictureNameList[0].replace('#', '_')
+            syncTimeStamp = int(pictureNameList[3][1:])
+            exp = str(pictureNameList[6][0:])
+            gain = str(pictureNameList[7][0:])
+
+            # 创建结构体
+            pictureInfo = PictureInfo()
+            info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1],exp,gain)
+
+            # 格式化字典一个key对应一个数组
+            pictureInfoDict.setdefault(deviceId, []).append(info)
+    
+    if 0 == frameFileCount:
+        print("initialize pictureInfoDict failed. Not found frame file")
+
+
+def matchFrame(pictureInfoDict):
+    global primaryId, frameRate
+    global streamProfileDict
+
+    # 获取比较图像数组,取第一个设备的Color数组来进行比较
+    compareList = []
+    for key in pictureInfoDict:
+        listTmp = list(pictureInfoDict[key])
+        for info in listTmp:
+            if info.deviceId == primaryId and info.sensorType == 'color':
+                compareList.append(info)
+
+    # 根据开流情况动态分析，减少循环次数
+    hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict
+    hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict
+    hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict
+    hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict
+    hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict
+
+    # 匹配相邻帧
+    resultDict = {}
+    consumedList = []
+    dictIndex = 0
+    frameTspHalfGap = int(1000.0/frameRate/2.0+0.5)
+    for comparePic in compareList:
+        resultDict.setdefault(dictIndex, []).append(comparePic)
+        for key in pictureInfoDict:
+            listTmp = list(pictureInfoDict[key])
+            # 对比彩色
+            if hasColorProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对Depth
+            if hasDepthProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # IR
+            if hasIRProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对左IR
+            if hasIRLeftProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+            # 对右IR
+            if hasIRRightProfile:
+                for info in listTmp:
+                    if info.deviceId == primaryId and info.sensorType == comparePic.sensorType:
+                        continue
+
+                    if info in consumedList:
+                        continue
+
+                    if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap:
+                        resultDict.setdefault(dictIndex, []).append(info)
+                        consumedList.append(info)
+                        break
+
+        # 计算偏差
+        minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp)
+        for info in resultDict[dictIndex]:
+            info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp
+
+        dictIndex += 1
+
+    return resultDict
+
+
+def safe_make_dir(path):
+    if not os.path.exists(path):
+        os.makedirs(path)
+
+
+def handleSyncFrames():
+    global resultPath
+    global sourceFramesPath
+    global streamProfileDict
+
+    fp = open(f"{resultPath}/frameMatchLog.txt", "w")
+
+    if not initProfileInfoDict():
+        print("Initialize stream profile dictionary failed. exit")
+        return
+    print(f"Stream profiles: {streamProfileDict}")
+    streamProfileCount = len(streamProfileDict.items())
+
+    pictureInfoDict = {}
+
+    initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict)
+    if len(pictureInfoDict) <= 0:
+        print("init pictureInfoDict failed. pictureInfoDict.len = 0")
+        return
+
+    # # Dump pictureInfoDict
+    # for key in pictureInfoDict:
+    #     print("===================key:%s" % key)
+    #     listTmp = list(pictureInfoDict[key])
+    # for info in listTmp:
+    #     print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath))
+
+    deviceCount = getDeviceCount()
+    print("=================device count:%d" % deviceCount)
+
+    resultDict = {}
+    resultDict = matchFrame(pictureInfoDict)
+
+    for key in resultDict:
+        fp.write("===================result key:%s\n" % key)
+        print("===================result key:%s" % key)
+        listTmp = list(resultDict[key])
+        for info in listTmp:
+            fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+            print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (
+                info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath))
+
+    matchPath = f"{resultPath}/matchFrames/"
+    safe_make_dir(matchPath)
+    notMatchPath = f"{resultPath}/notMatchFrames/"
+    abnormalPath = f"{resultPath}/abnormal/"
+
+    for key in resultDict:
+        listTmp = list(resultDict[key])
+        if (len(listTmp) == (deviceCount * streamProfileCount)):
+            haveAbnormalData = False
+            for info in listTmp:
+                if info.tspDiff >= tspRangeThreshold:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt
+                    shutil.copy(info.picturePath, path)
+                    haveAbnormalData = True
+                else:
+                    path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                        info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt
+                    shutil.copy(info.picturePath, path)
+
+            if haveAbnormalData:
+                safe_make_dir(abnormalPath)
+                for info in listTmp:
+                    if info.tspDiff >= tspRangeThreshold:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt 
+                        shutil.copy(info.picturePath, path)
+                    else:
+                        path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                            info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt
+                        shutil.copy(info.picturePath, path)
+        else:
+            safe_make_dir(notMatchPath)
+            for info in listTmp:
+                newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str(
+                    info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt
+                shutil.copy(info.picturePath, newFilePath)
+
+def main():
+    if not initPrimaryId():
+        print("Initialize primaryId failed. exit.")
+        return
+
+    if not initSyncConfigParams():
+        print("Initialized Sync config parameter failed. exit.")
+        return
+
+    initResultDir()
+
+    print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}")
+    handleSyncFrames()
+
+    return
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Devices'")
+    parser.add_argument('frames_dir', type=str, help="Frames directory")
+    parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory")
+    args = parser.parse_args()
+
+    sourceRootPath = args.frames_dir
+    sourceFramesPath = f"{args.frames_dir}/TotalModeFrames"
+    mainPythonWorkPath = args.main_script_root_dir
+    print(f"Start of CommonFrameMatch synchronize frame. sourceRootPath={sourceRootPath}")
+
+    main()
+
+    print("Finish of CommonFrameMatch synchronize frame.")

orbbec_camera/config/multicamera_sync/StreamProfileInfo.txt

@@ -0,0 +1,16 @@
+{
+	"D2CAlignMode":	"ALIGN_D2C_SW_MODE",
+	"streamProfiles":	[{
+			"sensorType":	"OB_SENSOR_COLOR",
+			"width":	640,
+			"height":	480,
+			"fps":	30,
+			"format":	"MJPEG"
+		}, {
+			"sensorType":	"OB_SENSOR_IR_LEFT",
+			"width":	640,
+			"height":	480,
+			"fps":	30,
+			"format":	"Y8"
+		}]
+}

orbbec_camera/tools/start_multi_sync.sh

@@ -0,0 +1,131 @@
+#!/bin/bash
+
+# Function to reset timestamp
+reset_timestamp() {
+    if ! ros2 service call /G330_0/set_reset_timestamp std_srvs/srv/SetBool '{data: true}'; then
+        printf "Error: Failed to publish start_capture message\n" >&2
+        return 1
+    fi
+    echo "Timestamp reset successfully."
+}
+
+# Function to sync interleaver laser
+sync_interleaverlaser() {
+    if ! ros2 service call /G330_0/set_sync_interleaverlaser orbbec_camera_msgs/srv/SetInt32 '{data: 0}'; then
+        printf "Error: Failed to launch Orbbec camera\n" >&2
+        return 1
+    fi
+}
+
+# Function to save image
+save_image() {
+    printf "Executing save_image.sh...\n"
+    if ! ros2 topic pub --once /start_capture std_msgs/msg/Int32 "{data: 100}"; then
+        printf "Error: Failed to publish start_capture message\n" >&2
+        return 1
+    fi
+    echo "Image saved successfully."
+}
+
+# Function to source the setup script
+setup_environment() {
+    local setup_script="install/setup.bash"
+    if [[ -f "$setup_script" ]]; then
+        source "$setup_script"
+        printf "Environment setup sourced successfully.\n"
+    else
+        printf "Error: Setup script %s not found\n" "$setup_script" >&2
+        return 1
+    fi
+}
+# Function to adjust USB memory settings
+set_usb_memory() {
+    local usb_memory_value=700
+    if ! sudo sh -c "echo $usb_memory_value > /sys/module/usbcore/parameters/usbfs_memory_mb"; then
+        printf "Error: Failed to set USB memory value\n" >&2
+        return 1
+    fi
+    printf "USB memory set to %d MB.\n" "$usb_memory_value"
+}
+# Function to run the Orbbec multi_save_rgbir_node command in the background
+run_multi_save_rgbir_node() {
+    ros2 run orbbec_camera multi_save_rgbir_node &
+    local pid=$!
+    if ! kill -0 "$pid" 2>/dev/null; then
+        printf "Error: Failed to run multi_save_rgbir_node\n" >&2
+        return 1
+    fi
+    printf "multi_save_rgbir_node is running in the background with PID %d.\n" "$pid"
+}
+# Function to launch ROS2 Orbbec camera with multi camera sync
+launch_orbbec_camera() {
+    if ! ros2 launch orbbec_camera multi_camera_synced.launch.py; then
+        printf "Error: Failed to launch Orbbec camera\n" >&2
+        return 1
+    fi
+    printf "Orbbec camera launched successfully.\n"
+}
+# Function to run camera
+run_camera() {
+    setup_environment || return 1
+    set_usb_memory || return 1
+    # Run multi_save_rgbir_node in the background and delay for 2 seconds before launching the camera
+    run_multi_save_rgbir_node || return 1
+    sleep 2
+    launch_orbbec_camera || return 1
+}
+
+
+# Function to display the menu
+display_menu() {
+    printf "\nSelect a function to run:\n"
+    printf "1. run_camera\n"
+    printf "2. reset_timestamp\n"
+    printf "3. sync_interleaverlaser\n"
+    printf "4. save_image\n"
+    printf "0. Exit\n"
+}
+
+# Function to execute the selected function
+run_function() {
+    case $1 in
+        1)
+            run_camera
+            ;;
+        2)
+            reset_timestamp
+            ;;
+        3)
+            sync_interleaverlaser
+            ;;
+        4)
+            save_image
+            ;;
+        0)
+            printf "Exiting the script...\n"
+            return 0
+            ;;
+        *)
+            printf "Invalid choice. Please select a valid option.\n" >&2
+            return 1
+            ;;
+    esac
+}
+
+# Main function to run the menu system
+main() {
+    setup_environment || return 1
+    while true; do
+        display_menu
+        read -rp "Enter your choice: " choice
+        if ! run_function "$choice"; then
+            continue
+        fi
+        if [ "$choice" -eq 0 ]; then
+            break
+        fi
+    done
+}
+
+# Call the main function
+main