zyworkflow 0.0.1__py3-none-any.whl

zyworkflow/data/collection.py

@@ -0,0 +1,1241 @@
+# coding: utf-8
+import os
+import cv2
+import csv
+import time
+import threading
+import re
+import numpy as np
+import concurrent.futures
+from collections import deque
+from typing import Optional, List, Tuple, Dict, Any
+
+from zyworkflow.utils.utils import *
+from zyworkflow.utils.pose import get_target_pose
+from zyworkflow.data.process import process_dataset
+from zyworkflow.utils.logger_config import setup_data_collection_logger
+logger = setup_data_collection_logger()
+
+
+class ImageCaptureThread:
+    def __init__(self, image_url: str, buffer_size: int = 200):
+        self.image_url = image_url
+        self.buffer_size = buffer_size
+        self.running = False
+        self.thread: Optional[threading.Thread] = None
+
+        self.image_buffer: deque = deque(maxlen=buffer_size)
+        self.buffer_lock = threading.RLock()
+
+        self.capture_count = 0
+        self.error_count = 0
+        self.last_capture_time = 0
+        self.capture_durations = deque(maxlen=100)
+
+        import requests
+        self._session = requests.Session()
+        adapter = requests.adapters.HTTPAdapter(
+            pool_connections=10,
+            pool_maxsize=50,
+            max_retries=2,
+            pool_block=False
+        )
+        self._session.mount('http://', adapter)
+        self._session.mount('https://', adapter)
+
+    def _get_image_with_session(self, image_url):
+        try:
+            response = self._session.get(image_url, timeout=0.5)
+            if response.status_code == 200:
+                img_array = np.frombuffer(response.content, np.uint8)
+                img = cv2.imdecode(img_array, cv2.IMREAD_COLOR)
+                return img, "Success"
+            else:
+                return None, f"HTTP {response.status_code}"
+        except Exception as e:
+            return None, str(e)
+
+    def start(self):
+        if self.running:
+            logger.warning("图像采集线程已经在运行")
+            return
+
+        self.running = True
+        self.thread = threading.Thread(
+            target=self._capture_loop,
+            name="ImageCaptureThread",
+            daemon=True
+        )
+        self.thread.start()
+        logger.info(f"图像采集线程启动，目标频率: 20Hz")
+
+    def stop(self):
+        self.running = False
+        if self._session:
+            self._session.close()
+        if self.thread and self.thread.is_alive():
+            self.thread.join(timeout=2.0)
+        logger.info(f"图像采集线程停止，共采集 {self.capture_count} 张图像，错误 {self.error_count} 次")
+
+    def _capture_loop(self):
+        capture_interval = 0.01
+        consecutive_errors = 0
+        max_consecutive_errors = 5
+
+        while self.running:
+            try:
+                start_time = time.perf_counter()
+                img, info = self._get_image_with_session(self.image_url)
+                if img is not None:
+                    timestamp = time.time()
+                    with self.buffer_lock:
+                        self.image_buffer.append((timestamp, img))
+                        self.capture_count += 1
+                        self.last_capture_time = timestamp
+                    consecutive_errors = 0
+
+                    capture_time = time.perf_counter() - start_time
+                    self.capture_durations.append(capture_time)
+                else:
+                    self.error_count += 1
+                    consecutive_errors += 1
+                    if consecutive_errors >= max_consecutive_errors:
+                        logger.error(f"连续采集失败 {consecutive_errors} 次: {info}")
+                        consecutive_errors = 0
+
+                process_time = time.perf_counter() - start_time
+                if process_time < capture_interval:
+                    sleep_time = capture_interval - process_time
+                    if sleep_time > 0.001:
+                        time.sleep(sleep_time * 0.9)
+                else:
+                    if process_time > capture_interval * 1.5:
+                        logger.warning(f"图像采集超时: {process_time * 1000:.1f}ms > {capture_interval * 1000:.1f}ms")
+
+                    time.sleep(0.001)
+
+            except Exception as e:
+                self.error_count += 1
+                logger.error(f"图像采集异常: {e}")
+                time.sleep(0.01)
+
+    def get_image_at(self, target_time: float, time_tolerance: float = 0.02):
+        with self.buffer_lock:
+            if not self.image_buffer:
+                return None
+
+            buffer_list = list(self.image_buffer)
+            left, right = 0, len(buffer_list) - 1
+            best_match = None
+            min_time_diff = float('inf')
+
+            while left <= right:
+                mid = (left + right) // 2
+                timestamp, img = buffer_list[mid]
+                time_diff = abs(timestamp - target_time)
+
+                if time_diff < min_time_diff:
+                    min_time_diff = time_diff
+                    best_match = (timestamp, img)
+
+                if timestamp < target_time:
+                    left = mid + 1
+                else:
+                    right = mid - 1
+
+            if best_match and min_time_diff <= time_tolerance:
+                return best_match[1]
+
+        return None
+
+    def get_latest_image(self):
+        with self.buffer_lock:
+            if self.image_buffer:
+                return self.image_buffer[-1]
+        return None
+
+    def clear_buffer(self):
+        with self.buffer_lock:
+            self.image_buffer.clear()
+        logger.debug("图像缓冲区已清空")
+
+    def get_stats(self) -> Dict:
+        with self.buffer_lock:
+            buffer_len = len(self.image_buffer)
+            recent_durations = list(self.capture_durations)
+
+        avg_duration = np.mean(recent_durations) * 1000 if recent_durations else 0
+        max_duration = np.max(recent_durations) * 1000 if recent_durations else 0
+
+        return {
+            'running': self.running,
+            'buffer_size': buffer_len,
+            'total_captured': self.capture_count,
+            'error_count': self.error_count,
+            'last_capture_time': self.last_capture_time,
+            'avg_capture_time_ms': f"{avg_duration:.1f}",
+            'max_capture_time_ms': f"{max_duration:.1f}",
+            'current_fps': self.capture_count / max(1, time.time() - self.last_capture_time) if self.capture_count > 0 else 0
+        }
+
+
+@register(name='bnn-pick')
+class Pick:
+    def __init__(self, ability_id, dataset_id, init_pose, speed=40, sampling_rate=20):
+        self.speed = speed
+        self.ability_id = ability_id
+        self.dataset_id = dataset_id
+        self.init_pose = init_pose
+        self._stop_requested = False
+        self._stop_lock = threading.Lock()
+        self.msg = {'code': 0, 'msg': 'Execution completed', 'action': {}}
+
+        self.recording = False
+        self.record_thread = None
+        self.image_thread = None
+        self.data_dir = os.path.join("/workspace/dataset", self.dataset_id, self.ability_id)
+
+        self.sampling_rate = sampling_rate
+        self.sampling_interval = 1.0 / self.sampling_rate
+
+        self.time_alignment_tolerance = 0.01
+
+        self._last_arm_status = None
+        self._last_arm_time = 0
+        self._arm_cache_duration = 0.05
+
+        self._stats = {
+            'total_samples': 0,
+            'arm_calls': 0,
+            'image_hits': 0,
+            'image_misses': 0,
+            'processing_times': deque(maxlen=100)
+        }
+
+        self._target_positions = []
+        self._target_reached = False
+        self._target_position_lock = threading.Lock()
+
+        os.makedirs(self.data_dir, exist_ok=True)
+
+        self.record_count = self._get_max_record_number()
+        logger.info(f"开始执行数据采集任务，任务名称: {self.dataset_id}-{self.ability_id}")
+
+        self._thread_pool = concurrent.futures.ThreadPoolExecutor(
+            max_workers=3,
+            thread_name_prefix="PickAsync"
+        )
+
+    def _get_max_record_number(self) -> int:
+        if not os.path.exists(self.data_dir):
+            return 0
+
+        max_num = 0
+        pattern = re.compile(r'traj_(\d{3})')
+
+        for item in os.listdir(self.data_dir):
+            item_path = os.path.join(self.data_dir, item)
+            if os.path.isdir(item_path):
+                match = pattern.match(item)
+                if match:
+                    num = int(match.group(1))
+                    if num > max_num:
+                        max_num = num
+
+        logger.debug(f"找到最大记录编号: {max_num}")
+        return max_num
+
+    def set_target_position(self, position):
+        with self._target_position_lock:
+            self._target_positions = position
+            self._target_reached = False
+
+    def mark_target_reached(self):
+        with self._target_position_lock:
+            self._target_reached = True
+
+    def _is_position_reached(self, current_pose, tolerance=0.01):
+        with self._target_position_lock:
+            if not self._target_positions:
+                return False
+
+            if self._target_reached:
+                return True
+
+            if len(current_pose) >= 3 and len(self._target_positions) >= 3:
+                dx = abs(current_pose[0] - self._target_positions[0])
+                dy = abs(current_pose[1] - self._target_positions[1])
+                dz = abs(current_pose[2] - self._target_positions[2])
+
+                distance = (dx ** 2 + dy ** 2 + dz ** 2) ** 0.5
+                reached = distance < tolerance
+
+                if reached:
+                    self._target_reached = True
+
+                return reached
+
+            return False
+
+    def _get_gripper_state(self, current_pose):
+        if self._is_position_reached(current_pose):
+            return 0, 0
+        else:
+            return 1000, 1000
+
+    def request_stop(self):
+        with self._stop_lock:
+            self._stop_requested = True
+            logger.debug(f"任务 {self.dataset_id}-{self.ability_id} 收到停止请求")
+
+    def clear_stop_flag(self):
+        with self._stop_lock:
+            self._stop_requested = False
+            logger.debug(f"任务 {self.dataset_id}-{self.ability_id} 停止标志已清除")
+
+    def should_stop(self):
+        with self._stop_lock:
+            return self._stop_requested
+
+    def preprocess_image(self, img):
+        if img is None:
+            return None
+        try:
+            img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
+            return img
+        except Exception as e:
+            logger.error(f"图像预处理失败: {e}")
+            return None
+
+    def _start_async_capture(self):
+        logger.info("启动异步采集线程...")
+
+        self.image_thread = ImageCaptureThread(rgb_image_url, buffer_size=300)
+        self.image_thread.start()
+
+        time.sleep(0.3)
+        logger.info("异步采集线程启动完成")
+
+    def _stop_async_capture(self):
+        logger.info("停止异步采集线程...")
+
+        if self.image_thread:
+            self.image_thread.stop()
+            self.image_thread = None
+
+        logger.info("异步采集线程停止完成")
+
+    def _get_arm_status_cached(self) -> Tuple[List[float], List[float]]:
+        current_time = time.time()
+
+        if (self._last_arm_status is None or
+                (current_time - self._last_arm_time) > self._arm_cache_duration):
+            try:
+                status, _ = get_arm_status()
+                if status:
+                    pose = status.get("pose", [0.0] * 6)
+                    joint_deg = status.get("joint", [0.0] * 6)
+                    joint_rad = [j / 180.0 * 3.1415926535 for j in joint_deg]
+
+                    self._last_arm_status = (pose, joint_rad)
+                    self._last_arm_time = current_time
+                    self._stats['arm_calls'] += 1
+
+                    return pose, joint_rad
+            except Exception as e:
+                logger.debug(f"获取机械臂状态失败: {e}")
+
+        if self._last_arm_status:
+            return self._last_arm_status
+
+        return [0.0] * 6, [0.0] * 6
+
+    def _record_trajectory_async(self, filename: str, sku: str, img_dir: str):
+        try:
+            logger.info(f"开始高性能记录: {filename}, SKU: {sku}")
+
+            import shutil
+            if os.path.exists(img_dir):
+                shutil.rmtree(img_dir)
+            os.makedirs(img_dir, exist_ok=True)
+
+            self._start_async_capture()
+            time.sleep(0.3)
+
+            csv_file = open(filename, "w", newline="")
+            csv_writer = csv.writer(csv_file)
+
+            csv_writer.writerow([
+                "Time(s)", "X(m)", "Y(m)", "Z(m)",
+                "Rx(rad)", "Ry(rad)", "Rz(rad)",
+                "j1(rad)", "j2(rad)", "j3(rad)",
+                "j4(rad)", "j5(rad)", "j6(rad)",
+                "Gripper_Set", "Gripper_Real", "SKU", "Image_Filename"
+            ])
+
+            start_time = time.perf_counter()
+            next_sample_time = start_time
+            sample_count = 0
+
+            image_queue = deque()
+            image_save_thread_running = True
+
+            def image_save_worker():
+                saved_count = 0
+                while image_save_thread_running or image_queue:
+                    if image_queue:
+                        img_path, image_data = image_queue.popleft()
+                        try:
+                            if image_data is not None:
+                                cv2.imwrite(img_path, image_data)
+                                saved_count += 1
+                        except Exception as e:
+                            logger.debug(f"保存图像失败 {img_path}: {e}")
+                    else:
+                        time.sleep(0.001)
+                logger.debug(f"图像保存线程结束，共保存 {saved_count} 张图像")
+
+            image_save_thread = threading.Thread(
+                target=image_save_worker,
+                name="ImageSaveThread",
+                daemon=True
+            )
+            image_save_thread.start()
+
+            logger.info(f"开始数据采集，目标采样率: {self.sampling_rate}Hz")
+
+            last_log_time = start_time
+            last_sample_count = 0
+
+            while self.recording and not self.should_stop():
+                current_time = time.perf_counter()
+
+                if current_time >= next_sample_time:
+                    loop_start = current_time
+                    try:
+                        rel_time = current_time - start_time
+
+                        pose_data, joint_data = self._get_arm_status_cached()
+                        gripper_set, gripper_real = self._get_gripper_state(pose_data)
+
+                        image_filename = ""
+                        if self.image_thread:
+                            image = self.image_thread.get_image_at(time.time(), self.time_alignment_tolerance)
+                            if image is not None:
+                                img_filename = f"{rel_time:.6f}.png"
+                                image_filename = img_filename
+                                img_path = os.path.join(img_dir, img_filename)
+
+                                image_queue.append((img_path, image))
+                                self._stats['image_hits'] += 1
+                            else:
+                                self._stats['image_misses'] += 1
+
+                        csv_writer.writerow([
+                            f"{rel_time:.6f}",
+                            *[f"{p:.6f}" for p in pose_data],
+                            *[f"{j:.6f}" for j in joint_data],
+                            f"{gripper_set}",
+                            f"{gripper_real}",
+                            f"{sku}",
+                            image_filename
+                        ])
+
+                        sample_count += 1
+                        self._stats['total_samples'] += 1
+                        if sample_count % 30 == 0:
+                            csv_file.flush()
+                            elapsed = time.perf_counter() - start_time
+                            actual_rate = sample_count / elapsed if elapsed > 0 else 0
+
+                            recent_samples = sample_count - last_sample_count
+                            recent_time = current_time - last_log_time
+                            recent_rate = recent_samples / recent_time if recent_time > 0 else 0
+
+                            logger.info(
+                                f"进度: {sample_count}样本, "
+                                f"时间: {elapsed:.1f}s, "
+                                f"平均率: {actual_rate:.1f}Hz, "
+                                f"近期率: {recent_rate:.1f}Hz, "
+                                f"图像命中: {self._stats['image_hits']}/{sample_count}"
+                            )
+
+                            last_log_time = current_time
+                            last_sample_count = sample_count
+
+                        next_sample_time = start_time + (sample_count * self.sampling_interval)
+                        process_time = time.perf_counter() - loop_start
+                        self._stats['processing_times'].append(process_time)
+
+                        if process_time > self.sampling_interval * 1.5:
+                            logger.warning(
+                                f"采样处理时间过长: {process_time * 1000:.1f}ms > "
+                                f"间隔{self.sampling_interval * 1000:.1f}ms"
+                            )
+                    except Exception as e:
+                        logger.error(f"采样过程出错: {e}")
+                        sample_count += 1
+                        next_sample_time = start_time + (sample_count * self.sampling_interval)
+
+                else:
+                    sleep_time = next_sample_time - time.perf_counter()
+                    if sleep_time > 0.0005:
+                        end_time = time.perf_counter() + sleep_time
+                        while time.perf_counter() < end_time:
+                            time.sleep(0.0001)
+
+            image_save_thread_running = False
+            if image_save_thread.is_alive():
+                image_save_thread.join(timeout=2.0)
+
+            self._stop_async_capture()
+            csv_file.flush()
+            csv_file.close()
+
+            total_time = time.perf_counter() - start_time
+            actual_rate = sample_count / total_time if total_time > 0 else 0
+
+            stats_file = filename.replace('.txt', '_stats.txt')
+            with open(stats_file, 'w') as sf:
+                sf.write("=== 轨迹记录统计 ===\n")
+                sf.write(f"总样本数: {sample_count}\n")
+                sf.write(f"总时间: {total_time:.3f}s\n")
+                sf.write(f"目标采样率: {self.sampling_rate}Hz\n")
+                sf.write(f"实际采样率: {actual_rate:.2f}Hz\n")
+                sf.write(f"机械臂调用次数: {self._stats['arm_calls']}\n")
+                sf.write(f"图像命中数: {self._stats['image_hits']}\n")
+                sf.write(f"图像缺失数: {self._stats['image_misses']}\n")
+                sf.write(f"SKU: {sku}\n")
+
+                if self._stats['processing_times']:
+                    avg_process = np.mean(self._stats['processing_times']) * 1000
+                    max_process = np.max(self._stats['processing_times']) * 1000
+                    sf.write(f"平均处理时间: {avg_process:.2f}ms\n")
+                    sf.write(f"最大处理时间: {max_process:.2f}ms\n")
+
+            logger.info(f"记录完成: {filename}")
+            logger.info(f"最终统计: {sample_count}样本, {total_time:.2f}s, {actual_rate:.1f}Hz")
+
+        except Exception as e:
+            logger.error(f"记录轨迹数据异常: {e}", exc_info=True)
+            self._stop_async_capture()
+            if 'csv_file' in locals() and not csv_file.closed:
+                csv_file.close()
+
+    def _start_recording(self, sku: str):
+        if self.recording:
+            logger.warning("已经在记录中，忽略重复开始")
+            return
+
+        self.recording = True
+        self.record_count += 1
+
+        self._stats = {
+            'total_samples': 0,
+            'arm_calls': 0,
+            'image_hits': 0,
+            'image_misses': 0,
+            'processing_times': deque(maxlen=100)
+        }
+
+        self._last_arm_status = None
+        self._last_arm_time = 0
+
+        with self._target_position_lock:
+            self._target_positions = []
+            self._target_reached = False
+
+        imgs_dir = f"images"
+        filename = f"actions.txt"
+        traj_dir = f"traj_{self.record_count:03d}"
+        filepath = os.path.join(self.data_dir, traj_dir, filename)
+        imgs_filepath = os.path.join(self.data_dir, traj_dir, imgs_dir)
+        os.makedirs(os.path.dirname(filepath), exist_ok=True)
+        os.makedirs(imgs_filepath, exist_ok=True)
+        self.record_thread = threading.Thread(
+            target=self._record_trajectory_async,
+            args=(filepath, sku, imgs_filepath),
+            name=f"RecordThread_{self.record_count}",
+            daemon=True
+        )
+        self.record_thread.start()
+
+        logger.info(f"开始第 {self.record_count} 次数据记录 -> {filepath}, SKU: {sku}")
+
+    def _stop_recording(self):
+        if not self.recording:
+            return
+
+        self.recording = False
+        if self.record_thread and self.record_thread.is_alive():
+            self.record_thread.join(timeout=5.0)
+
+        logger.info(f"第 {self.record_count} 次数据记录完成")
+
+    def run_from_http_camera(self, sku):
+        try:
+            logger.info(f"执行{self.dataset_id}-{self.ability_id}任务")
+
+            self.set_sampling_rate(20.0)
+            start_time = time.time()
+            self.clear_stop_flag()
+
+            code, _ = post_arm_movej(self.init_pose, speed=self.speed)
+            time.sleep(2)
+
+            dof_time = time.time()
+            xyz, quat_xyzw, _ = post_6dof(str(sku))
+            logger.info(f'{self.dataset_id}-{self.ability_id} - 6dof 用时{time.time() - dof_time}')
+
+            if xyz is None:
+                logger.info(f"{self.dataset_id}-{self.ability_id}任务执行完成，没检测到6dof信息")
+                return {'code': -4, 'msg': "没检测到6dof信息", 'dataset_id': self.dataset_id, 'traj_path': ''}
+
+            coords = xyz
+            quat = quat_xyzw
+            seven_dim_coord = np.concatenate((coords, quat))
+
+            state, _ = get_arm_status()
+            end_pose = state["pose"]
+            logger.info(f'起始位姿: {end_pose}')
+
+            pos_target = get_target_pose(end_pose, seven_dim_coord)
+            pos_target = [float(pos) for pos in pos_target]
+            logger.info(f'pos_target: {pos_target}')
+
+            move_posy = [state["pose"][0], pos_target[1], state["pose"][2],
+                         state["pose"][3], state["pose"][4], state["pose"][5]]
+            move_posz = [pos_target[0] + 0.3, pos_target[1], pos_target[2],
+                         state["pose"][3], state["pose"][4], state["pose"][5]]
+            move_posx = [pos_target[0] + 0.17, pos_target[1], pos_target[2],
+                         state["pose"][3], state["pose"][4], state["pose"][5]]
+
+            self.set_target_position(move_posx)
+            self._start_recording(sku)
+            time.sleep(0.5)
+
+            code, _ = post_arm_movel(move_posy, speed=30)
+            code, _ = post_arm_movel(move_posz, speed=self.speed)
+            code, _ = post_arm_movel(move_posx, speed=self.speed)
+
+            self.mark_target_reached()
+            post_gripper_move(0)
+
+            keep_time = 2
+            keep_start = time.time()
+            while (time.time() - keep_start) < keep_time and not self.should_stop():
+                time.sleep(0.1)
+
+            self._stop_recording()
+
+            time.sleep(1)
+            grip_state, set_position, position, _ = get_gripper_status()
+            if grip_state and position:
+                if grip_state == 3 or position == 0:
+                    logger.info('---------检测到物体掉落---------')
+                    return {'code': -5, 'msg': "没有夹到物体", 'dataset_id': self.dataset_id, 'traj_path': ''}
+
+            post_gripper_move(1000)
+            state, _ = get_arm_status()
+            pose = state['pose']
+            pose[0] = pose[0] + 0.2
+            code, _ = post_arm_movel(pose, speed=self.speed)
+            code, _ = post_arm_movej(self.init_pose, speed=self.speed)
+
+            total_time = time.time() - start_time
+            logger.info(f'{self.dataset_id}-{self.ability_id} - 总用时{total_time}')
+            logger.info(f"{self.dataset_id}-{self.ability_id}任务执行完成，机械臂已回到初始位置")
+            traj_path = os.path.join(self.data_dir, f"traj_{self.record_count:03d}")
+            process_dataset(self.data_dir, self.record_count)
+            self.msg['traj_path'] = traj_path
+            self.msg['dataset_id'] = self.dataset_id
+            self.msg['ability_id'] = self.ability_id
+            return self.msg
+
+        except RuntimeError as e:
+            msg = {'code': -1, 'msg': str(e), 'dataset_id': self.dataset_id, 'traj_path': '', 'ability_id': self.ability_id}
+            self.msg = msg
+            self._stop_recording()
+            return self.msg
+
+        except Exception as e:
+            msg = {'code': -1, 'msg': f"未知错误: {str(e)}", 'dataset_id': self.dataset_id, 'traj_path': '', 'ability_id': self.ability_id}
+            self.msg = msg
+            self._stop_recording()
+            return self.msg
+
+        finally:
+            if self.msg.get('code') != 0:
+                traj_dir_to_clean = os.path.join(self.data_dir, f"traj_{self.record_count:03d}")
+                if os.path.isdir(traj_dir_to_clean):
+                    try:
+                        import shutil
+                        shutil.rmtree(traj_dir_to_clean)
+                        logger.warning(f"{self.dataset_id}-{self.ability_id} 采集失败，已清理目录: {traj_dir_to_clean}")
+                    except Exception as e:
+                        logger.error(f"{self.dataset_id}-{self.ability_id} 清理失败目录 {traj_dir_to_clean} 时出错: {e}")
+
+            self._stop_recording()
+            if hasattr(self, '_thread_pool'):
+                self._thread_pool.shutdown(wait=False)
+
+    def set_time_alignment_tolerance(self, tolerance_ms: float):
+        self.time_alignment_tolerance = tolerance_ms / 1000.0
+        logger.info(f"时间对齐容差设置为: {tolerance_ms}ms")
+
+    def set_sampling_rate(self, rate_hz: float):
+        if rate_hz <= 0:
+            logger.warning(f"无效的采样率: {rate_hz} Hz，使用默认值20Hz")
+            rate_hz = 20.0
+
+        self.sampling_rate = rate_hz
+        self.sampling_interval = 1.0 / rate_hz
+        logger.info(f"采样率设置为: {rate_hz} Hz (间隔: {self.sampling_interval:.6f} 秒)")
+
+    def get_capture_stats(self) -> Dict:
+        stats = {
+            'recording': self.recording,
+            'record_count': self.record_count,
+            'sampling_rate': self.sampling_rate,
+            'time_alignment_tolerance_ms': self.time_alignment_tolerance * 1000,
+            'performance_stats': self._stats.copy()
+        }
+
+        if self.image_thread:
+            stats['image_capture'] = self.image_thread.get_stats()
+
+        if self._stats['processing_times']:
+            proc_times = list(self._stats['processing_times'])
+            stats['avg_processing_time_ms'] = np.mean(proc_times) * 1000
+            stats['max_processing_time_ms'] = np.max(proc_times) * 1000
+
+        return stats
+
+    def get_record_count(self) -> int:
+        return self.record_count
+
+    def cleanup(self):
+        logger.info("开始清理Pick类资源...")
+        self._stop_recording()
+        if hasattr(self, '_thread_pool'):
+            self._thread_pool.shutdown(wait=False)
+        logger.info("Pick类资源清理完成")
+
+
+@register(name='bnn-place')
+class Place:
+    def __init__(self, ability_id, dataset_id, init_pose, speed=40, sampling_rate=20):
+        self.speed = speed
+        self.ability_id = ability_id
+        self.dataset_id = dataset_id
+        self.init_pose = init_pose
+        self._stop_requested = False
+        self._stop_lock = threading.Lock()
+        self.msg = {'code': 0, 'msg': 'Execution completed', 'action': {}}
+
+        self.recording = False
+        self.record_thread = None
+        self.image_thread = None
+        self.data_dir = os.path.join("/workspace/dataset", self.dataset_id, self.ability_id)
+
+        self.sampling_rate = sampling_rate
+        self.sampling_interval = 1.0 / self.sampling_rate
+
+        self.time_alignment_tolerance = 0.01
+        self._last_arm_status = None
+        self._last_arm_time = 0
+        self._arm_cache_duration = 0.05
+
+        self._stats = {
+            'total_samples': 0,
+            'arm_calls': 0,
+            'image_hits': 0,
+            'image_misses': 0,
+            'processing_times': deque(maxlen=100)
+        }
+
+        self._target_positions = []
+        self._target_reached = False
+        self._target_position_lock = threading.Lock()
+
+        os.makedirs(self.data_dir, exist_ok=True)
+
+        self.record_count = self._get_max_record_number()
+        logger.info(f"开始执行数据采集任务，任务名称: {self.dataset_id}-{self.ability_id}")
+
+        self._thread_pool = concurrent.futures.ThreadPoolExecutor(
+            max_workers=3,
+            thread_name_prefix="PickAsync"
+        )
+
+    def _get_max_record_number(self) -> int:
+        if not os.path.exists(self.data_dir):
+            return 0
+
+        max_num = 0
+        pattern = re.compile(r'traj_(\d{3})')
+
+        for item in os.listdir(self.data_dir):
+            item_path = os.path.join(self.data_dir, item)
+            if os.path.isdir(item_path):
+                match = pattern.match(item)
+                if match:
+                    num = int(match.group(1))
+                    if num > max_num:
+                        max_num = num
+
+        logger.debug(f"找到最大记录编号: {max_num}")
+        return max_num
+
+    def set_target_position(self, position):
+        with self._target_position_lock:
+            self._target_positions = position
+            self._target_reached = False
+
+    def mark_target_reached(self):
+        with self._target_position_lock:
+            self._target_reached = True
+
+    def _is_position_reached(self, current_pose, tolerance=0.01):
+        with self._target_position_lock:
+            if not self._target_positions:
+                return False
+
+            if self._target_reached:
+                return True
+
+            if len(current_pose) >= 3 and len(self._target_positions) >= 3:
+                dx = abs(current_pose[0] - self._target_positions[0])
+                dy = abs(current_pose[1] - self._target_positions[1])
+                dz = abs(current_pose[2] - self._target_positions[2])
+
+                distance = (dx ** 2 + dy ** 2 + dz ** 2) ** 0.5
+                reached = distance < tolerance
+
+                if reached:
+                    self._target_reached = True
+
+                return reached
+
+            return False
+
+    def _get_gripper_state(self, current_pose):
+        if self._is_position_reached(current_pose):
+            return 1000, 1000
+        else:
+            return 0, 0
+
+    def request_stop(self):
+        with self._stop_lock:
+            self._stop_requested = True
+            logger.debug(f"任务 {self.task_id} 收到停止请求")
+
+    def clear_stop_flag(self):
+        with self._stop_lock:
+            self._stop_requested = False
+            logger.debug(f"任务 {self.task_id} 停止标志已清除")
+
+    def should_stop(self):
+        with self._stop_lock:
+            return self._stop_requested
+
+    def preprocess_image(self, img):
+        if img is None:
+            return None
+        try:
+            img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
+            return img
+        except Exception as e:
+            logger.error(f"图像预处理失败: {e}")
+            return None
+
+    def _start_async_capture(self):
+        logger.info("启动异步采集线程...")
+
+        self.image_thread = ImageCaptureThread(rgb_image_url, buffer_size=300)
+        self.image_thread.start()
+
+        time.sleep(0.3)
+        logger.info("异步采集线程启动完成")
+
+    def _stop_async_capture(self):
+        logger.info("停止异步采集线程...")
+
+        if self.image_thread:
+            self.image_thread.stop()
+            self.image_thread = None
+
+        logger.info("异步采集线程停止完成")
+
+    def _get_arm_status_cached(self) -> Tuple[List[float], List[float]]:
+        current_time = time.time()
+
+        if (self._last_arm_status is None or
+                (current_time - self._last_arm_time) > self._arm_cache_duration):
+            try:
+                status, _ = get_arm_status()
+                if status:
+                    pose = status.get("pose", [0.0] * 6)
+                    joint_deg = status.get("joint", [0.0] * 6)
+                    joint_rad = [j / 180.0 * 3.1415926535 for j in joint_deg]
+
+                    self._last_arm_status = (pose, joint_rad)
+                    self._last_arm_time = current_time
+                    self._stats['arm_calls'] += 1
+
+                    return pose, joint_rad
+            except Exception as e:
+                logger.debug(f"获取机械臂状态失败: {e}")
+
+        if self._last_arm_status:
+            return self._last_arm_status
+
+        return [0.0] * 6, [0.0] * 6
+
+    def _record_trajectory_async(self, filename: str, sku: str, img_dir: str):
+        try:
+            logger.info(f"开始高性能记录: {filename}, SKU: {sku}")
+
+            import shutil
+            if os.path.exists(img_dir):
+                shutil.rmtree(img_dir)
+            os.makedirs(img_dir, exist_ok=True)
+
+            self._start_async_capture()
+            time.sleep(0.3)
+
+            csv_file = open(filename, "w", newline="")
+            csv_writer = csv.writer(csv_file)
+
+            csv_writer.writerow([
+                "Time(s)", "X(m)", "Y(m)", "Z(m)",
+                "Rx(rad)", "Ry(rad)", "Rz(rad)",
+                "j1(rad)", "j2(rad)", "j3(rad)",
+                "j4(rad)", "j5(rad)", "j6(rad)",
+                "Gripper_Set", "Gripper_Real", "SKU", "Image_Filename"
+            ])
+
+            start_time = time.perf_counter()
+            next_sample_time = start_time
+            sample_count = 0
+
+            image_queue = deque()
+            image_save_thread_running = True
+
+            def image_save_worker():
+                saved_count = 0
+                while image_save_thread_running or image_queue:
+                    if image_queue:
+                        img_path, image_data = image_queue.popleft()
+                        try:
+                            if image_data is not None:
+                                cv2.imwrite(img_path, image_data)
+                                saved_count += 1
+                        except Exception as e:
+                            logger.debug(f"保存图像失败 {img_path}: {e}")
+                    else:
+                        time.sleep(0.001)
+                logger.debug(f"图像保存线程结束，共保存 {saved_count} 张图像")
+
+            image_save_thread = threading.Thread(
+                target=image_save_worker,
+                name="ImageSaveThread",
+                daemon=True
+            )
+            image_save_thread.start()
+
+            logger.info(f"开始数据采集，目标采样率: {self.sampling_rate}Hz")
+
+            last_log_time = start_time
+            last_sample_count = 0
+
+            while self.recording and not self.should_stop():
+                current_time = time.perf_counter()
+
+                if current_time >= next_sample_time:
+                    loop_start = current_time
+
+                    try:
+                        rel_time = current_time - start_time
+                        pose_data, joint_data = self._get_arm_status_cached()
+                        gripper_set, gripper_real = self._get_gripper_state(pose_data)
+
+                        image_filename = ""
+                        if self.image_thread:
+                            image = self.image_thread.get_image_at(time.time(), self.time_alignment_tolerance)
+                            if image is not None:
+                                img_filename = f"{rel_time:.6f}.png"
+                                image_filename = img_filename
+                                img_path = os.path.join(img_dir, img_filename)
+
+                                image_queue.append((img_path, image))
+                                self._stats['image_hits'] += 1
+                            else:
+                                self._stats['image_misses'] += 1
+
+                        csv_writer.writerow([
+                            f"{rel_time:.6f}",
+                            *[f"{p:.6f}" for p in pose_data],
+                            *[f"{j:.6f}" for j in joint_data],
+                            f"{gripper_set}",
+                            f"{gripper_real}",
+                            f"{sku}",
+                            image_filename
+                        ])
+
+                        sample_count += 1
+                        self._stats['total_samples'] += 1
+
+                        if sample_count % 30 == 0:
+                            csv_file.flush()
+                            elapsed = time.perf_counter() - start_time
+                            actual_rate = sample_count / elapsed if elapsed > 0 else 0
+
+                            recent_samples = sample_count - last_sample_count
+                            recent_time = current_time - last_log_time
+                            recent_rate = recent_samples / recent_time if recent_time > 0 else 0
+
+                            logger.info(
+                                f"进度: {sample_count}样本, "
+                                f"时间: {elapsed:.1f}s, "
+                                f"平均率: {actual_rate:.1f}Hz, "
+                                f"近期率: {recent_rate:.1f}Hz, "
+                                f"图像命中: {self._stats['image_hits']}/{sample_count}"
+                            )
+
+                            last_log_time = current_time
+                            last_sample_count = sample_count
+
+                        next_sample_time = start_time + (sample_count * self.sampling_interval)
+                        process_time = time.perf_counter() - loop_start
+                        self._stats['processing_times'].append(process_time)
+
+                        if process_time > self.sampling_interval * 1.5:
+                            logger.warning(
+                                f"采样处理时间过长: {process_time * 1000:.1f}ms > "
+                                f"间隔{self.sampling_interval * 1000:.1f}ms"
+                            )
+
+                    except Exception as e:
+                        logger.error(f"采样过程出错: {e}")
+                        sample_count += 1
+                        next_sample_time = start_time + (sample_count * self.sampling_interval)
+
+                else:
+                    sleep_time = next_sample_time - time.perf_counter()
+                    if sleep_time > 0.0005:
+                        end_time = time.perf_counter() + sleep_time
+                        while time.perf_counter() < end_time:
+                            time.sleep(0.0001)
+
+            image_save_thread_running = False
+            if image_save_thread.is_alive():
+                image_save_thread.join(timeout=2.0)
+
+            self._stop_async_capture()
+
+            csv_file.flush()
+            csv_file.close()
+
+            total_time = time.perf_counter() - start_time
+            actual_rate = sample_count / total_time if total_time > 0 else 0
+
+            stats_file = filename.replace('.txt', '_stats.txt')
+            with open(stats_file, 'w') as sf:
+                sf.write("=== 轨迹记录统计 ===\n")
+                sf.write(f"总样本数: {sample_count}\n")
+                sf.write(f"总时间: {total_time:.3f}s\n")
+                sf.write(f"目标采样率: {self.sampling_rate}Hz\n")
+                sf.write(f"实际采样率: {actual_rate:.2f}Hz\n")
+                sf.write(f"机械臂调用次数: {self._stats['arm_calls']}\n")
+                sf.write(f"图像命中数: {self._stats['image_hits']}\n")
+                sf.write(f"图像缺失数: {self._stats['image_misses']}\n")
+                sf.write(f"SKU: {sku}\n")
+
+                if self._stats['processing_times']:
+                    avg_process = np.mean(self._stats['processing_times']) * 1000
+                    max_process = np.max(self._stats['processing_times']) * 1000
+                    sf.write(f"平均处理时间: {avg_process:.2f}ms\n")
+                    sf.write(f"最大处理时间: {max_process:.2f}ms\n")
+
+            logger.info(f"记录完成: {filename}")
+            logger.info(f"最终统计: {sample_count}样本, {total_time:.2f}s, {actual_rate:.1f}Hz")
+
+        except Exception as e:
+            logger.error(f"记录轨迹数据异常: {e}", exc_info=True)
+            self._stop_async_capture()
+            if 'csv_file' in locals() and not csv_file.closed:
+                csv_file.close()
+
+    def _start_recording(self, sku: str):
+        if self.recording:
+            logger.warning("已经在记录中，忽略重复开始")
+            return
+
+        self.recording = True
+        self.record_count += 1
+
+        self._stats = {
+            'total_samples': 0,
+            'arm_calls': 0,
+            'image_hits': 0,
+            'image_misses': 0,
+            'processing_times': deque(maxlen=100)
+        }
+
+        self._last_arm_status = None
+        self._last_arm_time = 0
+
+        with self._target_position_lock:
+            self._target_positions = []
+            self._target_reached = False
+
+        imgs_dir = f"images"
+        filename = f"actions.txt"
+        traj_dir = f"traj_{self.record_count:03d}"
+        filepath = os.path.join(self.data_dir, traj_dir, filename)
+        imgs_filepath = os.path.join(self.data_dir, traj_dir, imgs_dir)
+        os.makedirs(os.path.dirname(filepath), exist_ok=True)
+        os.makedirs(imgs_filepath, exist_ok=True)
+
+        self.record_thread = threading.Thread(
+            target=self._record_trajectory_async,
+            args=(filepath, sku, imgs_filepath),
+            name=f"RecordThread_{self.record_count}",
+            daemon=True
+        )
+        self.record_thread.start()
+        logger.info(f"开始第 {self.record_count} 次数据记录 -> {filepath}, SKU: {sku}")
+
+    def _stop_recording(self):
+        if not self.recording:
+            return
+
+        self.recording = False
+
+        if self.record_thread and self.record_thread.is_alive():
+            self.record_thread.join(timeout=5.0)
+        logger.info(f"第 {self.record_count} 次数据记录完成")
+
+    def run_from_http_camera(self, sku):
+        try:
+            logger.info(f"执行{self.dataset_id}-{self.ability_id}任务")
+            start_time = time.time()
+            self.set_sampling_rate(20.0)
+            self.clear_stop_flag()
+            dof_time = time.time()
+            xyz, quat_xyzw, _ = post_find_basket()
+            logger.info(f'{self.dataset_id}-{self.ability_id} - find_basket 用时{time.time() - dof_time}')
+            if xyz is None:
+                logger.info(f"{self.dataset_id}-{self.ability_id}任务执行完成，没检测到basket信息")
+                return {'code': -4, 'msg': "没检测到basket信息", 'dataset_id': self.dataset_id, 'traj_path': ''}
+            coords = xyz
+            quat = quat_xyzw
+            seven_dim_coord = np.concatenate((coords, quat))
+
+            state, _ = get_arm_status()
+            end_pose = state["pose"]
+            logger.info(f'起始位姿: {end_pose}')
+
+            pos_target = get_target_pose(end_pose, seven_dim_coord)
+            pos_target = [float(pos) for pos in pos_target]
+            logger.info(f'pos_target: {pos_target}')
+
+            move_posy = [state["pose"][0], pos_target[1], state["pose"][2], state["pose"][3], state["pose"][4],
+                         state["pose"][5]]
+            move_posx = [pos_target[0] + 0.17, pos_target[1], pos_target[2] + 0.35, state["pose"][3], state["pose"][4],
+                         state["pose"][5]]
+
+            self.set_target_position(move_posx)
+            self._start_recording(sku)
+            time.sleep(0.5)
+
+            code, _ = post_arm_movel(move_posy, speed=self.speed)
+            code, _ = post_arm_movel(move_posx, speed=self.speed)
+
+            self.mark_target_reached()
+            post_gripper_move(1000)
+
+            keep_time = 2
+            keep_start = time.time()
+            while (time.time() - keep_start) < keep_time and not self.should_stop():
+                time.sleep(0.1)
+
+            self._stop_recording()
+            time.sleep(1)
+
+            code, _ = post_arm_movej(self.init_pose, speed=self.speed)
+            # post_gripper_move(0)
+            logger.info(f'{self.dataset_id}-{self.ability_id} - 总用时{time.time() - start_time}')
+            logger.info(f"{self.dataset_id}-{self.ability_id}任务执行完成，机械臂已回到初始位置")
+            traj_path = os.path.join(self.data_dir, f"traj_{self.record_count:03d}")
+            process_dataset(self.data_dir, self.record_count)
+            self.msg['traj_path'] = traj_path
+            self.msg['dataset_id'] = self.dataset_id
+            self.msg['ability_id'] = self.ability_id
+            return self.msg
+        except RuntimeError as e:
+            msg = {'code': -1, 'msg': str(e), 'dataset_id': self.dataset_id, 'traj_path': '', 'ability_id': self.ability_id}
+            self.msg = msg
+            return self.msg
+        except Exception as e:
+            msg = {'code': -1, 'msg': f"未知错误: {str(e)}", 'dataset_id': self.dataset_id, 'traj_path': '', 'ability_id': self.ability_id}
+            self.msg = msg
+            return self.msg
+
+        finally:
+            if self.msg.get('code') != 0:
+                traj_dir_to_clean = os.path.join(self.data_dir, f"traj_{self.record_count:03d}")
+                if os.path.isdir(traj_dir_to_clean):
+                    try:
+                        import shutil
+                        shutil.rmtree(traj_dir_to_clean)
+                        logger.warning(f"{self.dataset_id}-{self.ability_id} 采集失败，已清理目录: {traj_dir_to_clean}")
+                    except Exception as e:
+                        logger.error(f"{self.dataset_id}-{self.ability_id} 清理失败目录 {traj_dir_to_clean} 时出错: {e}")
+
+            self._stop_recording()
+            if hasattr(self, '_thread_pool'):
+                self._thread_pool.shutdown(wait=False)
+
+    def set_time_alignment_tolerance(self, tolerance_ms: float):
+        self.time_alignment_tolerance = tolerance_ms / 1000.0
+        logger.info(f"时间对齐容差设置为: {tolerance_ms}ms")
+
+    def set_sampling_rate(self, rate_hz: float):
+        if rate_hz <= 0:
+            logger.warning(f"无效的采样率: {rate_hz} Hz，使用默认值20Hz")
+            rate_hz = 20.0
+
+        self.sampling_rate = rate_hz
+        self.sampling_interval = 1.0 / rate_hz
+        logger.info(f"采样率设置为: {rate_hz} Hz (间隔: {self.sampling_interval:.6f} 秒)")
+
+    def get_capture_stats(self) -> Dict:
+        stats = {
+            'recording': self.recording,
+            'record_count': self.record_count,
+            'sampling_rate': self.sampling_rate,
+            'time_alignment_tolerance_ms': self.time_alignment_tolerance * 1000,
+            'performance_stats': self._stats.copy()
+        }
+
+        if self.image_thread:
+            stats['image_capture'] = self.image_thread.get_stats()
+
+        if self._stats['processing_times']:
+            proc_times = list(self._stats['processing_times'])
+            stats['avg_processing_time_ms'] = np.mean(proc_times) * 1000
+            stats['max_processing_time_ms'] = np.max(proc_times) * 1000
+
+        return stats
+
+    def get_record_count(self) -> int:
+        return self.record_count
+
+    def cleanup(self):
+        logger.info("开始清理Pick类资源...")
+        self._stop_recording()
+        if hasattr(self, '_thread_pool'):
+            self._thread_pool.shutdown(wait=False)
+        logger.info("Pick类资源清理完成")