import time import logging # 配置日志 logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') logger = logging.getLogger('pika_device') def get_serial_ports(vidpid='1a86:7522'): """ 搜索所有指定vidpid的串口 vidpid: 指定设备的VID:PID字符串, 默认值为'1a86:7522' 返回找到的所有符合的串口号列表 """ from serial.tools import list_ports ports = list_ports.comports() pika_ports = [] for port in ports: if port.vid is not None and port.pid is not None: if '{:04x}:{:04x}'.format(port.vid, port.pid) == vidpid: pika_ports.append(port.device) # else: # print('pidvid:', '{:04x}:{:04x}'.format(port.vid, port.pid)) return pika_ports def check_pika_device(port): """ 检测串口对应的Pika设备类型 返回值: -1: 无法打开串口 0: 不是Pika设备 1: Pika Sense设备 2: Pika Gripper设备 """ import serial try: ser = serial.Serial( port=port, baudrate=460800, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=1.0 ) time.sleep(0.5) # 等待串口稳定 data = b'' expired_time = time.monotonic() + 1.0 # 最多等待1秒 while time.monotonic() < expired_time: if ser.in_waiting > 0: data += ser.read(ser.in_waiting) if len(data) > 200: # 足够的数据来判断 break time.sleep(0.05) ser.close() data_str = data.decode('utf-8', errors='ignore') if '"Command"' in data_str or '"AS5047"' in data_str or '"IMU"' in data_str: # logger.info('✓ 检测到 Pika Sense 设备: {}'.format(port)) return 1 elif '"motor"' in data_str or '"motorstatus"' in data_str: # logger.info('✓ 检测到 Pika Gripper 设备: {}'.format(port)) return 2 else: # logger.info('✗ 未检测到 Pika 设备: {}, 数据长度: {}'.format(port, len(data))) return 0 except: pass return -1 class PikaDevice(object): # _instance = None # _pika_sense_port = None # _pika_gripper_port = None # _lock = threading.Lock() PIKA_DEVICE_MAP = {} def __init__(self, dev_type=1, **kwargs): """ port: serial port dev_type: 1: sense, 2: gripper """ if dev_type not in [1, 2, 3]: raise ValueError('不支持dev_type={}'.format(dev_type)) self._dev_type = dev_type self._pika_sense_port = kwargs.get('pika_sense_port', None) self._pika_gripper_port = kwargs.get('pika_gripper_port', None) use_pika_sense = self._dev_type in [1, 3] use_pika_gripper = self._dev_type in [2, 3] self._pika_sense = None self._pika_gripper = None if (use_pika_sense and self._pika_sense_port is None) or (use_pika_gripper and self._pika_gripper_port is None): pika_ports = get_serial_ports() if not pika_ports: logger.error('未找到Pika设备, 请检查连接') exit(1) for port in pika_ports: device_type = check_pika_device(port) if device_type == 1 and use_pika_sense and self._pika_sense_port is None: self._pika_sense_port = port logger.info('✓ 检测到 Pika Sense 设备: {}'.format(port)) if not use_pika_gripper: break if device_type == 2 and use_pika_gripper and self._pika_gripper_port is None: self._pika_gripper_port = port logger.info('✓ 检测到 Pika Gripper 设备: {}'.format(port)) if not use_pika_sense: break if use_pika_sense and self._pika_sense_port is None: logger.error('未找到Pika Sense设备, 请检查连接') exit(1) if use_pika_gripper and self._pika_gripper_port is None: logger.error('未找到Pika Gripper设备, 请检查连接') exit(1) if use_pika_sense: print('Pika Sense设备:', self._pika_sense_port) if use_pika_gripper: print('Pika Gripper 设备:', self._pika_gripper_port) self.pika_tracker_device = None # def __new__(cls, *args, **kwargs): # if not cls._instance: # with cls._lock: # if not cls._instance: # cls._instance = super().__new__(cls) # cls._instance.init(*args, *kwargs) # return cls._instance def __del__(self): if self._pika_sense: self._pika_sense.disconnect() if self._pika_gripper: self._pika_gripper.disconnect() @property def pika_sense(self): if self._dev_type not in [1, 3]: return None if self._pika_sense is None: if self._pika_sense_port in self.PIKA_DEVICE_MAP: self._pika_sense = self.PIKA_DEVICE_MAP[self._pika_sense_port] return self._pika_sense from pika.sense import Sense # 初始化Sense对象 self._pika_sense = Sense(port=self._pika_sense_port) # 连接设备 if not self._pika_sense.connect(): logger.error('连接Pika Sense设备失败') exit(1) logger.info('Pika Sense设备连接成功') self.PIKA_DEVICE_MAP[self._pika_sense_port] = self._pika_sense # 注册共享 # 配置Vive Tracker（可选） # sense.set_vive_tracker_config(config_path='path/to/config', lh_config='lighthouse_config') tracker = self._pika_sense.get_vive_tracker() if not tracker: logger.error('Vive Tracker初始化失败') self._pika_sense.disconnect() exit(1) logger.info('Vive Tracker初始化成功') time.sleep(2) devices = self._pika_sense.get_tracker_devices() if not devices: logger.error('未检测到Vive Tracker设备') self._pika_sense.disconnect() exit(1) logger.info('检测到Vive Tracker设备: {}'.format(devices)) self.pika_tracker_device = None for device in devices: if device.startswith('WM'): self.pika_tracker_device = device break else: self.pika_tracker_device = devices[0] logger.info('开始跟踪设备: {}\n'.format(self.pika_tracker_device)) return self._pika_sense @property def pika_gripper(self): if self._dev_type not in [2, 3]: return None if self._pika_gripper is None: if self._pika_gripper_port in self.PIKA_DEVICE_MAP: self._pika_gripper = self.PIKA_DEVICE_MAP[self._pika_gripper_port] return self._pika_gripper from pika.gripper import Gripper self._pika_gripper = Gripper(port=self._pika_gripper_port) self.PIKA_DEVICE_MAP[self._pika_gripper_port] = self._pika_gripper # 注册共享 # 连接设备 if not self._pika_gripper.connect(): logger.error('连接Pika Gripper设备失败') if self._dev_type in [1, 3]: self.pika_sense.disconnect() exit(1) logger.info('Pika Gripper设备连接成功') return self._pika_gripper if __name__ == '__main__': pika_device1 = PikaDevice(1) pika_device1.pika_sense pika_device1.pika_gripper time.sleep(3) # input('=================') pika_device2 = PikaDevice(2) pika_device2.pika_sense pika_device2.pika_gripper input('=================') print(pika_device1) print(pika_device1.pika_sense) print(pika_device1.pika_gripper) print(pika_device2) print(pika_device2.pika_sense) print(pika_device2.pika_gripper) input('=================')