''' @Author : liujunshen @Ide : vscode @File : ruishou.py @Time : 2023/03/28 16:54:03 ''' import logging from socket import socket import struct import threading import time from typing import Optional from core.peripheral.hand.base import PeripheralHandBase from settings.config import settings hand_config = settings.config["hand_peripheral_parameter"] logger = logging.getLogger(__name__) def reconnect_decorator(func): """重新连接装饰器""" def inner(self, *args, **kwargs): try: return func(self, *args, **kwargs) except Exception: self.close() self._start_client() return func(self, *args, **kwargs) return inner class Constants: """睿手相关常量""" CMD_LOCATION = 3 HANDSHAKE_CMD = 0x01 HEARTBEAT_CMD = 0x02 SET_CURRENT_CMD = 0x03 MOTION_CONTROL_CMD = 0x04 DRAFTING_ACTION_CMD = 0x05 FINISH_ACTION_CMD = 0x06 class SendPckLocation: """睿手发送信号功能对于位置""" HANDSHAKE_VERSION_H = 4 HANDSHAKE_VERSION_L = 5 SET_CURRENT_CMD = 4 SET_CURRENT_CHANNEL = 5 SET_CURRENT_VALUE = 6 MOTION_CONTROL_HAND = 4 MOTION_CONTROL_THUMB_BENDING = 5 MOTION_CONTROL_INDEX_FINGER_BENDING = 6 MOTION_CONTROL_MIDDLE_FINGER_BENDING = 7 MOTION_CONTROL_RING_FINGER_BENDING = 8 MOTION_CONTROL_LITTLE_FINGER_BENDING = 9 MOTION_CONTROL_DURATION = 10 DRAFTING_ACTION_HAND = 4 DRAFTING_ACTION_IS_ELECTRIC = 5 DRAFTING_ACTION_CHANNELS = 6 DRAFTING_ACTION_CHANNEL_A_VALUE = 7 DRAFTING_ACTION_CHANNEL_B_VALUE = 8 DRAFTING_ACTION_DURATION = 9 class RecvPckLocation: """睿手接收信号功能对于位置""" HANDSHAKE_STATUS = 4 HANDSHAKE_REASON = 5 SET_CURRENT_CHANNEL = 5 SET_CURRENT_VALUE = 6 MOTION_CONTROL_STATUS = 4 MOTION_CONTROL_DURATION = 5 DRAFTING_ACTION_STATUS = 4 DRAFTING_ACTION_DURATION = 5 FINISH_ACTION_STATUS = 4 FINISH_ACTION_DURATION = 5 class RecvStatus: """睿手接收信号功能响应状态""" HANDSHAKE_SUCCESS = 0x00 HANDSHAKE_FAIL = 0x01 HANDSHAKE_REASON_OTHER = 0x00 HANDSHAKE_REASON_DIFF_VERSION = 0x01 class PckValue: """睿手数据包值""" BOTH_HANDS = 0x01 LEFT_HAND = 0x02 RIGHT_HAND = 0x03 class RuishouConnector: """睿手客户端 功能：连接；发送心跳包；开启线程发送信号；接收信号等 """ def __init__(self) -> None: self.__host = hand_config["hand_host"] self.__port = hand_config["hand_port"] self.__heart_interval = hand_config["hand_heart"] self.__hand_version = hand_config["hand_version"] self.__addr = (self.__host, self.__port) self.protocol = Protocol() self.sock = None self.is_connected = False def start_client(self): """启动客户端""" version_parm = { Constants.SendPckLocation.HANDSHAKE_VERSION_H: self.__hand_version[0], Constants.SendPckLocation.HANDSHAKE_VERSION_L: self.__hand_version[1] } sock = socket() # 链接服务端地址 logger.info("Connecting to and hand peripheral...") self.sock = sock self.sock.connect(self.__addr) logger.info("Hand Peripheral connected successfully.") # TODO: 连接失败的logger self.sync_send_data("handshake", version_parm) self.is_connected = True logger.info("handshake...") # 向服务端发送心跳包 send_heartbeat_t = threading.Thread(target=self.__send_beat_data, args=()) # recv_t.setDaemon(True) # TODO: 目前启动线程接收会导致同步接口无法接受到数据(接收线程已接收), 后续可考虑上锁解决 # recv_t.start() send_heartbeat_t.start() def create_send_t(self, send_data): """外部程序调用""" send_t = threading.Thread(target=self.send_data, args=(send_data,)) send_t.start() send_t.join() def send_data(self, cmd, update=None): if update is None: update = dict() send_data = self.protocol.get_pck(cmd, update) self.sock.sendall(send_data) def sync_send_data(self, cmd, update=None): """同步接口""" if update is None: update = dict() send_data = self.protocol.get_pck(cmd, update) if send_data: self.sock.send(send_data) res = self.filter_recv_msg() return res else: return None def filter_recv_msg(self): times = 0 res = {"msg": "fail"} while times <= 50: recv = self.sock.recv(1024) recv_ls = self.protocol.unpack_bytes(recv) for recv in recv_ls: res = self.protocol.parse_bytes(recv) if res["cmd"] != 2: return res times += 1 return res def __send_beat_data(self): try: while True: self.is_connected = True self.send_data("heartbeat") time.sleep(self.__heart_interval) except Exception: logger.info( "Send beat data failed, Hand Peripheral may be disconnected.") self.is_connected = False def close_client(self): if self.sock: self.sock.close() self.is_connected = False class Protocol: """睿手封装/解析包""" def __init__(self) -> None: self.pck_map = { "handshake": [ 0xAC, 0xAD, 0x05, 0x01, None, None, 0xFF, 0xFF, None, None ], "heartbeat": [ 0xAC, 0xAD, 0x05, 0x02, 0xFF, 0xFF, 0xFF, 0xFF, None, None ], "default_current": [ 0xAC, 0xAD, 0x05, 0x03, None, None, None, 0xFF, None, None ], "motion_control": [ 0xAC, 0xAD, 0x08, 0x04, None, None, None, None, None, None, None, None, None ], "drafting_action": [ 0xAC, 0xAD, 0x07, 0x05, None, None, None, None, None, None, None, None ], "finish_action": [ 0xAC, 0xAD, 0x05, 0x06, 0xFF, 0xFF, 0xFF, 0xFF, None, None ] } # 映射结果命令对应结果文本 self.recv_map = { "handshake": { "byte0": { 0x00: "success", 0x01: "fail" }, "byte1": { 0x00: "other", 0x01: "diff version" } }, "heartbeat": {""} } self.recv_head = (0xAE, 0xAF) # 睿手端数据包头 @staticmethod def cal_checksum(data): b_checksum = sum(data).to_bytes(2, byteorder="big") byte_h, byte_l = struct.unpack("2B", b_checksum) return byte_h, byte_l def get_pck(self, cmd, update_pck=None) -> Optional[bytearray]: """根据命令，参数更新包组装数据包 Args: cmd: 命令 ['handshake', 'heartbeat', 'default_current', 'motion_control', 'drafting_action', 'finish_action'] update_pck: 更新数据：电流参数等 Returns:符合协议的bytes指令 """ if update_pck is None: update_pck = dict() arr = self.pck_map.get(cmd, None) if not arr: return None for key in update_pck.keys(): if key > len(arr): return None arr[key] = update_pck[key] if None in arr[:-2]: return None bs = bytearray(0) byte_h, byte_l = self.cal_checksum(arr[3:-2]) arr[-2], arr[-1] = byte_h, byte_l for i in arr: bs += bytearray(i.to_bytes(1, byteorder="little")) return bs def unpack_bytes(self, recv_bytes) -> list: """将socket.recv数据按包头进行拆包 Args: recv_bytes: 接收的数据包 Returns: 分割后的命令数组 """ recv_list = struct.unpack_from(f"{len(recv_bytes)}B", recv_bytes) res_list = [] for idx in range(len(recv_list) - 1): # 寻找包头 if recv_list[idx] == self.recv_head[0] and (recv_list[idx + 1] == self.recv_head[1]): head_idx = idx data_len = recv_list[head_idx + 2] + 5 res_list.append(recv_list[head_idx:data_len + head_idx]) return res_list def parse_bytes(self, recv_nums) -> dict: """解析封装结果信息 Args: recv_nums: 单个数据包数组 Returns:封装后的结果信息 """ res = dict() res["cmd"] = recv_nums[Constants.CMD_LOCATION] if recv_nums[Constants.CMD_LOCATION] == Constants.HANDSHAKE_CMD: res["status"] = recv_nums[ Constants.RecvPckLocation.HANDSHAKE_STATUS] res["reason"] = recv_nums[ Constants.RecvPckLocation.HANDSHAKE_REASON] elif recv_nums[Constants.CMD_LOCATION] == Constants.SET_CURRENT_CMD: res["current_channel"] = recv_nums[ Constants.RecvPckLocation.SET_CURRENT_CHANNEL] res["current_val"] = recv_nums[ Constants.RecvPckLocation.SET_CURRENT_VALUE] elif recv_nums[Constants.CMD_LOCATION] == Constants.MOTION_CONTROL_CMD: res["motion_control_status"] = recv_nums[ Constants.RecvPckLocation.MOTION_CONTROL_STATUS] res["motion_control_duration"] = recv_nums[ Constants.RecvPckLocation.MOTION_CONTROL_DURATION] elif recv_nums[Constants.CMD_LOCATION] == Constants.DRAFTING_ACTION_CMD: res["drafting_action_status"] = recv_nums[ Constants.RecvPckLocation.DRAFTING_ACTION_STATUS] res["drafting_action_duration"] = recv_nums[ Constants.RecvPckLocation.DRAFTING_ACTION_DURATION] elif recv_nums[Constants.CMD_LOCATION] == Constants.FINISH_ACTION_CMD: res["drafting_action_status"] = recv_nums[ Constants.RecvPckLocation.FINISH_ACTION_STATUS] res["drafting_action_duration"] = recv_nums[ Constants.RecvPckLocation.FINISH_ACTION_DURATION] return res class RuishouClient(PeripheralHandBase): def __init__(self) -> None: self.connector = RuishouConnector() self.version = (0x01, 0x00) # 睿手版本 self.model = None def _start_client(self, version=None): """启动连接""" if self.connector.is_connected: return 1, "already connect" if not version: version = self.version try: self.connector.start_client() return 1, "success connect" except ConnectionRefusedError as e: return 0, f"fail, {e}" def _set_current(self, channel, val): """调节预设电流""" if not self.connector.is_connected: return 0 parm_d = dict() parm_d[Constants.SendPckLocation.SET_CURRENT_CHANNEL] = channel parm_d[Constants.SendPckLocation.SET_CURRENT_VALUE] = val parm_d[Constants.SendPckLocation.SET_CURRENT_CMD] = 1 # 开始 self.connector.sync_send_data(cmd="default_current", update=parm_d) parm_d[Constants.SendPckLocation.SET_CURRENT_CMD] = 3 # 调节 self.connector.sync_send_data(cmd="default_current", update=parm_d) parm_d[Constants.SendPckLocation.SET_CURRENT_CMD] = 2 # 结束 self.connector.sync_send_data(cmd="default_current", update=parm_d) return 1 @staticmethod def _change_hand_data(hand): if hand == "双手": return Constants.PckValue.BOTH_HANDS if hand == "左手": return Constants.PckValue.LEFT_HAND if hand == "右手": return Constants.PckValue.RIGHT_HAND @reconnect_decorator def _control_motion(self, grabbing_param): logger.info("Launch peripheral...") parm_d = { Constants.SendPckLocation.MOTION_CONTROL_HAND: self._change_hand_data(grabbing_param.hand_select), Constants.SendPckLocation.MOTION_CONTROL_THUMB_BENDING: grabbing_param.thumb, Constants.SendPckLocation.MOTION_CONTROL_INDEX_FINGER_BENDING: grabbing_param.index_finger, Constants.SendPckLocation.MOTION_CONTROL_MIDDLE_FINGER_BENDING: grabbing_param.middle_finger, Constants.SendPckLocation.MOTION_CONTROL_RING_FINGER_BENDING: grabbing_param.ring_finger, Constants.SendPckLocation.MOTION_CONTROL_LITTLE_FINGER_BENDING: grabbing_param.little_finger, Constants.SendPckLocation.MOTION_CONTROL_DURATION: grabbing_param.duration } res = self.connector.sync_send_data(cmd="motion_control", update=parm_d) logger.info("Launch peripheral success") return res @reconnect_decorator def _drafting_action(self, drafting_param): if not self.connector.is_connected: return 0 parm_d = { Constants.SendPckLocation.DRAFTING_ACTION_HAND: drafting_param.hand_select, Constants.SendPckLocation.DRAFTING_ACTION_IS_ELECTRIC: drafting_param.is_electric, Constants.SendPckLocation.DRAFTING_ACTION_CHANNELS: drafting_param.draft_channel, Constants.SendPckLocation.DRAFTING_ACTION_CHANNEL_A_VALUE: drafting_param.a_channel_value, Constants.SendPckLocation.DRAFTING_ACTION_CHANNEL_B_VALUE: drafting_param.b_channel_value, Constants.SendPckLocation.DRAFTING_ACTION_DURATION: drafting_param.duration } res = self.connector.sync_send_data(cmd="motion_control", update=parm_d) return res def init(self): _, msg = self._start_client() ret = {"is_connected": self.connector.is_connected, "msg": msg} return ret def set_db_model(self, db_model): self.model = db_model def start(self, train): ret_msg = self._control_motion(train.hand_peripherals) return ret_msg def stop(self): res = self.connector.sync_send_data(cmd="finish_action") return res def status(self): status = {"is_connected": self.connector.is_connected} return status def close(self): self.connector.close_client() ret = {"is_connected": False} return ret