add realman shadow src

realman_src/realman_aloha/shadow_rm_aloha/test/mu_test.py

@@ -0,0 +1,49 @@
+import multiprocessing as mp
+import time
+
+def collect_data(arm_id, cam_id, data_queue, lock):
+    while True:
+        # 模拟数据采集
+        arm_data = f"Arm {arm_id} data"
+        cam_data = f"Cam {cam_id} data"
+        
+        # 获取当前时间戳
+        timestamp = time.time()
+        
+        # 将数据放入队列
+        with lock:
+            data_queue.put((timestamp, arm_data, cam_data))
+        
+        # 模拟高帧率
+        time.sleep(0.01)
+
+def main():
+    num_arms = 4
+    num_cams = 4
+    
+    # 创建队列和锁
+    data_queue = mp.Queue()
+    lock = mp.Lock()
+    
+    # 创建进程
+    processes = []
+    for i in range(num_arms):
+        p = mp.Process(target=collect_data, args=(i, i, data_queue, lock))
+        processes.append(p)
+        p.start()
+    
+    # 主进程处理数据
+    try:
+        while True:
+            if not data_queue.empty():
+                with lock:
+                    timestamp, arm_data, cam_data = data_queue.get()
+                    print(f"Timestamp: {timestamp}, {arm_data}, {cam_data}")
+    except KeyboardInterrupt:
+        for p in processes:
+            p.terminate()
+        for p in processes:
+            p.join()
+
+if __name__ == "__main__":
+    main()

realman_src/realman_aloha/shadow_rm_aloha/test/test_aloha_data_synchronizer.py

@@ -0,0 +1,38 @@
+import os
+import shutil
+from datetime import datetime
+from shadow_rm_aloha.data_sub_process.aloha_data_synchronizer import DataCollector
+
+def test_create_dataset_dir():
+    # 设置测试参数
+    dataset_dir = './test_data/dataset'
+    dataset_name = 'test_episode'
+    max_timesteps = 100
+    camera_names = ['cam1', 'cam2']
+    overwrite = False
+
+    # 清理旧的测试数据
+    if os.path.exists(dataset_dir):
+        shutil.rmtree(dataset_dir)
+
+    # 创建 DataCollector 实例并调用 create_dataset_dir
+    collector = DataCollector(dataset_dir, dataset_name, max_timesteps, camera_names, overwrite)
+
+    # 检查目录是否按预期创建
+    date_str = datetime.now().strftime("%Y%m%d")
+    expected_dir = os.path.join(dataset_dir, date_str)
+    assert os.path.exists(expected_dir), f"Expected directory {expected_dir} does not exist."
+
+    # 检查文件名是否按预期递增
+    expected_file = os.path.join(expected_dir, dataset_name + '.hdf5')
+    assert collector.dataset_path == expected_file, f"Expected file path {expected_file}, but got {collector.dataset_path}"
+
+    # 再次调用 create_dataset_dir，检查文件名是否递增
+    # collector.create_dataset_dir()
+    expected_file_incremented = os.path.join(expected_dir, dataset_name + '_1.hdf5')
+    assert collector.dataset_path == expected_file_incremented, f"Expected file path {expected_file_incremented}, but got {collector.dataset_path}"
+
+    print("All tests passed.")
+
+if __name__ == '__main__':
+    test_create_dataset_dir()

realman_src/realman_aloha/shadow_rm_aloha/test/udp_test.py

@@ -0,0 +1,105 @@
+
+import multiprocessing
+import time
+import random
+import socket
+import json
+import logging
+
+# 设置日志级别
+logging.basicConfig(
+    level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
+)
+
+
+class test_udp():
+    def __init__(self):
+        arm_ip = '192.168.1.19'
+        arm_port = 8080
+        self.arm =socket.socket()
+        self.arm.connect((arm_ip, arm_port))
+        set_udp = {"command":"set_realtime_push","cycle":1,"enable":True,"port":8090,"ip":"192.168.1.101","custom":{"aloha_state":True,"joint_speed":True,"arm_current_status":True,"hand":False, "expand_state":True}}
+        self.arm.send(json.dumps(set_udp).encode('utf-8'))
+        state = self.arm.recv(1024)
+
+        logging.info(f"Send data to {arm_ip}:{arm_port}: {state}")
+
+        self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+        # 设置套接字选项，允许端口复用
+        self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+
+        local_ip = "192.168.1.101"
+        local_port = 8090  
+        self.udp_socket.bind((local_ip, local_port))
+        self.BUFFER_SIZE = 1024
+
+
+
+    def set_udp(self):
+
+        while True:
+            start_time = time.time()
+            data, addr = self.udp_socket.recvfrom(self.BUFFER_SIZE)
+            # 将接收到的UDP数据解码并解析为JSON
+            data = json.loads(data.decode('utf-8'))
+            end_time = time.time()
+            print(f"Received data {data}")        
+
+        udp_socket.close()
+
+
+def collect_arm_data(arm_id, queue, event):
+    while True:
+        data = f"Arm {arm_id} data {random.random()}"
+        queue.put((arm_id, data))
+        event.set()
+        time.sleep(1)
+
+def collect_camera_data(camera_id, queue, event):
+    while True:
+        data = f"Camera {camera_id} data {random.random()}"
+        queue.put((camera_id, data))
+        event.set()
+        time.sleep(1)
+
+def main():
+    arm_queues = [multiprocessing.Queue() for _ in range(4)]
+    camera_queues = [multiprocessing.Queue() for _ in range(4)]
+    arm_events = [multiprocessing.Event() for _ in range(4)]
+    camera_events = [multiprocessing.Event() for _ in range(4)]
+
+    arm_processes = [multiprocessing.Process(target=collect_arm_data, args=(i, arm_queues[i], arm_events[i])) for i in range(4)]
+    camera_processes = [multiprocessing.Process(target=collect_camera_data, args=(i, camera_queues[i], camera_events[i])) for i in range(4)]
+
+    for p in arm_processes + camera_processes:
+        p.start()
+
+    try:
+        while True:
+            for event in arm_events + camera_events:
+                event.wait()
+
+            for i in range(4):
+                if not arm_queues[i].empty():
+                    arm_id, arm_data = arm_queues[i].get()
+                    print(f"Received from Arm {arm_id}: {arm_data}")
+                    arm_events[i].clear()
+
+                if not camera_queues[i].empty():
+                    camera_id, camera_data = camera_queues[i].get()
+                    print(f"Received from Camera {camera_id}: {camera_data}")
+                    camera_events[i].clear()
+
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        for p in arm_processes + camera_processes:
+            p.terminate()
+
+if __name__ == "__main__":
+    main()
+
+
+# if __name__ == "__main__":
+#     test_udp = test_udp()
+#     test_udp.set_udp()