diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 58f8627..f0add3d 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -1,49 +1,140 @@ -from hardware.dobot_nova5 import dobot_nova5 -from hardware.force_sensor import XjcSensor - -from algorithms.arm_state import ArmState -from algorithms.controller_manager import ControllerManager -from algorithms.admittance_controller import AdmittanceController -from algorithms.position_controller import PositionController - -from tools.log import CustomLogger -from tools.yaml_operator import read_yaml -from tools.Rate import Rate +from functools import wraps +import sys import numpy as np import threading import time +from typing import List, Literal, Union + +from scipy.interpolate import interp1d + +import requests import os from scipy.spatial.transform import Rotation as R import ctypes libc = ctypes.CDLL("libc.so.6") import atexit -from tools.Trajectory import TrajectoryInterpolator +import copy + from scipy.spatial.transform import Slerp + + +try: + # 导入算法 + from .algorithms.arm_state import ArmState + from .algorithms.controller_manager import ControllerManager + from .algorithms.admittance_controller import AdmittanceController + from .algorithms.hybrid_controller import HybridController + from .algorithms.admithybrid_controller import AdmitHybridController + from .algorithms.position_controller import PositionController + from .algorithms.hybridPid_controller import HybridPidController + from .algorithms.hybridAdmit_controller import HybridAdmitController + from .algorithms.positionerSensor_controller import PositionerSensorController + from .algorithms.admittanceZ_controller import AdmittanceZController + from .algorithms.interpolation import linear_interpolate,spline_interpolate,circle_trajectory,cloud_point_interpolate,oscillation_wrench_interpolate,linear_wrench_interpolate + from .algorithms.force_planner import ForcePlanner + + # 导入硬件 + from .hardware.dobot_nova5 import dobot_nova5 + from .hardware.force_sensor import XjcSensor + + # 导入工具 + from .tools.log import CustomLogger + from .tools.Rate import Rate + from .tools.yaml_operator import read_yaml + from .tools.decorator_tools import custom_decorator,apply_decorators + from .tools.serial_tools import find_serial_by_location + from .tools.Trajectory import TrajectoryInterpolator +except: + #导入算法 + from algorithms.arm_state import ArmState + from algorithms.controller_manager import ControllerManager + from algorithms.admittance_controller import AdmittanceController + from algorithms.admittanceZ_controller import AdmittanceZController + from algorithms.hybrid_controller import HybridController + from algorithms.admithybrid_controller import AdmitHybridController + from algorithms.position_controller import PositionController + from algorithms.hybridPid_controller import HybridPidController + from algorithms.hybridAdmit_controller import HybridAdmitController + from algorithms.positionerSensor_controller import PositionerSensorController + from algorithms.interpolation import linear_interpolate,spline_interpolate,circle_trajectory,cloud_point_interpolate,oscillation_wrench_interpolate,linear_wrench_interpolate + from algorithms.force_planner import ForcePlanner + + # 导入硬件 + from hardware.dobot_nova5 import dobot_nova5 + from hardware.force_sensor import XjcSensor + + # 导入工具 + from tools.log import CustomLogger + from tools.Rate import Rate + from tools.yaml_operator import read_yaml + from tools.decorator_tools import custom_decorator,apply_decorators + from tools.serial_tools import find_serial_by_location + from tools.Trajectory import TrajectoryInterpolator + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + +current_file_path = os.path.abspath(__file__) +MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path))) +print(MassageRobot_Dobot_Path) +# 添加目标文件夹到系统路径 +sys.path.append(MassageRobot_Dobot_Path) +from VortXDB.client import VTXClient + +""" +在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。 +当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。 +""" + +def track_function(function_name,log = False): + def before(self, *args, **kwargs): + self.current_function = function_name + if log: + self.logger.log_info(f"Entering function: {function_name}") + def after(self, result, *args, **kwargs): + self.current_function = None + if log: + self.logger.log_info(f"Exiting function: {function_name},code: {result}") + return custom_decorator(before=before, after=after) + +@apply_decorators class MassageRobot: def __init__(self,arm_config_path,is_log=False): - self.logger = CustomLogger( - log_name="测试日志", - log_file="logs/MassageRobot_nova5_test.log", - precise_time=True) + self.logger = CustomLogger() if is_log: - self.logger_data = CustomLogger(log_name="运动数据日志", - log_file="logs/MassageRobot_kinematics_data.log", - precise_time=True) + self.logger_data = CustomLogger() # 日志线程 threading.Thread(target=self.log_thread,daemon=True).start() + + self.vtxdb = VTXClient() self.arm_state = ArmState() self.arm_config = read_yaml(arm_config_path) # arm 实例化时机械臂类内部进行通讯连接 - self.arm = dobot_nova5(self.arm_config['arm_ip']) + self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip']) self.arm.start() + + self.thermotherapy = None + self.shockwave = None + self.stone = None + self.heat = None + self.ion = None + self.force_plan = ForcePlanner() # 传感器 + # self.force_sensor = None self.force_sensor = XjcSensor() - self.force_sensor.connect() - # 控制器初始化(初始化为导纳控制) + # self.force_sensor.connect() + # 控制器,初始为导纳控制 self.controller_manager = ControllerManager(self.arm_state) self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0]) - self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1]) + self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1]) + self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2]) + self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3]) + self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4]) + self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5]) + self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6]) + self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7]) + self.controller_manager.switch_controller('admittance') # 按摩头参数暂时使用本地数据 massage_head_dir = self.arm_config['massage_head_dir'] @@ -84,8 +175,8 @@ class MassageRobot: self.skip_envent = threading.Event() self.skip_envent.clear() # 跳过初始化为False # 按摩调整 - self.adjust_wrench_event = threading.Event() - self.adjust_wrench_event.clear() # 调整初始化为False + self.adjust_wrench_envent = threading.Event() + self.adjust_wrench_envent.clear() # 调整初始化为False self.pos_increment = np.zeros(3,dtype=np.float64) self.adjust_wrench = np.zeros(6,dtype=np.float64) @@ -123,6 +214,114 @@ class MassageRobot: # --- self.last_time = -1 self.cur_time = -1 + + + def sensor_set_zero(self): + self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position() + arm_orientation_set0 = self.arm_orientation_set0 + print("arm_orientation_set0",R.from_quat(arm_orientation_set0).as_euler('xyz',degrees=True)) + self.b_rotation_s_set0 = R.from_quat(arm_orientation_set0).as_matrix() + ######### + max_retries = 5 + #no_head_wrong=np.array([-80.01506042, 116.34187317, -87.65788269, -0.32910481, -0.65792173, -0.61110526])#旧版传感器） + head_set0=np.array([0, 0, 0, 0, 0, 0])#新版传感器） + min_tolerance=0.001 + retry_count = 0 + self.force_sensor.disable_active_transmission() + time.sleep(0.5) + while retry_count < max_retries: + # 调用读取数据的函数，假设返回值是一个 numpy 数组 + data = self.force_sensor.read_data_f32() + data = self.force_sensor.read_data_f32() + data = self.force_sensor.read_data_f32() + + # 检查返回值是否为 np.array 类型 + if isinstance(data, np.ndarray): + # 判断数据是否接近目标值 + + if np.allclose(data, head_set0, atol=min_tolerance): + print(f"检测到力传感器已置零：{data}") + break + else: + print(f"检测到力传感器未置零：{data}") + + # 禁用传感器传输 + self.force_sensor.disable_active_transmission() + self.force_sensor.disable_active_transmission() + time.sleep(0.5) + + # 尝试设置传感器清零 + code = self.force_sensor.set_zero() + code = self.force_sensor.set_zero() + max_try = 3 + + while code != 0 and max_try > 0: + self.logger.log_error("Force sensor set zero failed, retrying...") + self.force_sensor.disable_active_transmission() + self.force_sensor.disable_active_transmission() + time.sleep(0.5) + code = self.force_sensor.set_zero() + code = self.force_sensor.set_zero() + max_try -= 1 + time.sleep(0.5) + + # 如果多次尝试后失败 + if max_try == 0: + self.logger.log_error("Force sensor set zero failed, exiting...") + requests.post( + "http://127.0.0.1:5000/on_message", data={"message": "传感器初始化失败"} + ) + requests.post( + "http://127.0.0.1:5000/update_massage_status", data={"massage_service_started": False} + ) + # sys.exit(0) + return -1 # 置零失败 + + # 成功后跳出主循环 + break + + else: + self.logger.log_error("读取数据失败或格式错误，尝试重试...") + + # 增加重试次数 + retry_count += 1 + self.force_sensor.disable_active_transmission() + self.force_sensor.disconnect() + time.sleep(1) + self.force_sensor = XjcSensor() + time.sleep(1) # 每次重试之间添加一个短暂的延迟 + + if retry_count == max_retries: + self.logger.log_error(f"超过最大重试次数 ({max_retries})，操作失败，退出程序...") + requests.post( + "http://127.0.0.1:5000/on_message", data={"message": "传感器初始化失败"} + ) + requests.post( + "http://127.0.0.1:5000/update_massage_status", data={"massage_service_started": False} + ) + return -2 #读取失败 + + return 0 #读取成功并置零成功 + + def sensor_enable(self): + code = self.force_sensor.enable_active_transmission() + max_try = 3 + while code!= 0 and max_try > 0: + self.logger.log_error("Force sensor enable_active_transmission failed,retrying...") + code = self.force_sensor.enable_active_transmission() + max_try -= 1 + time.sleep(0.1) + if max_try == 0: + self.logger.log_error("Force sensor enable_active_transmission failed,exiting...") + requests.post( + "http://127.0.0.1:5000/on_message", data={"message": "传感器初始化失败"} + ) + requests.post( + "http://127.0.0.1:5000/update_massage_status", data={"massage_service_started": False} + ) + # sys.exit(0) + return -1 + return 0 # 预测步骤 def kalman_predict(self,x, P, Q): @@ -131,7 +330,7 @@ class MassageRobot: # 预测误差协方差 P_predict = P + Q return x_predict, P_predict - + # 更新步骤 def kalman_update(self,x_predict, P_predict, z, R): # 卡尔曼增益 @@ -441,6 +640,919 @@ class MassageRobot: force = rot_matrix.T @ temp_force torque = rot_matrix.T @ np.cross(temp_force,vector_p) + rot_matrix.T @ torque return np.concatenate([force, torque]) + + @track_function("set_position",True) + def set_position(self,pose,is_wait = False): + self.logger.log_info(f"set postion:{pose}") + x,y,z = pose[0],pose[1],pose[2] + roll,pitch,yaw = pose[3],pose[4],pose[5] + pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]]) + time.sleep(0.1) + code = self.arm.RunPoint_P_inPose_M_RAD(poses_list = pose_command) + time.sleep(0.1) + return code + + @track_function("move_to_point",True) + def move_to_point(self, t, pose, is_interrupt=True, wait=True, timeout=0.5, interpolation:Literal["linear","circle","cloud_point"] ='linear', algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance",is_switch_controller = False): + """ + 移动到指定的点 + param: + t: 时间或时间序列 + pose(nx6): 世界坐标系下的位姿或位姿序列,当输入为None时,表示保持当前位姿 + is_interrupt: 是否允许中断 + wait: 是否等待运动完成 + timeout: 超时时间 + interpolation: 插值方式 + algorithm: 控制算法 + return: + code: 0表示成功 + 1表示超时 + 2表示用户打断 + 3表示硬件错误 + 4表示参数错误 + """ + self.move_to_point_count += 1 + self.logger.log_blue(f"move_to_point_count: {self.move_to_point_count}") + self.logger.log_yellow(f"move_to_point: {pose[-1]}") + # 在函数内部直接规划轨迹 + traj = self.traj_generate(t, pose, None, interpolation=interpolation) + if traj is None: + self.logger.log_error("轨迹未生成或生成错误") + return 4 + + self.logger.log_yellow(f"traj_quat: {R.from_quat(traj['quat'][-1]).as_euler('xyz',degrees=False)}") + self.logger.log_yellow(f"traj_pos: {traj['pos'][-1]}") + + time_seq = traj['t'] + pos_traj = traj['pos'] + quat_traj = traj['quat'] + + if is_switch_controller == True: + try: + self.controller_manager.switch_controller(algorithm) + except ValueError as error: + self.logger.log_error(error) + self.logger.log_error("切换到%s控制器失败" % algorithm) + return 4 + self.logger.log_info("切换到%s控制器" % algorithm) + + # self.logger.log_info("当前为%s控制器" % algorithm) + + for i in range(len(time_seq)): + if self.interrupt_event.is_set(): + self.interrupt_event.clear() + self.logger.log_error("self.interrupt_event.clear()") + self.arm_state.desired_position = copy.deepcopy(self.arm_state.arm_position) + self.arm_state.desired_orientation = copy.deepcopy(self.arm_state.arm_orientation) + massage_wrench = np.array([0, 0, 0, 0, 0, 0]) + self.arm_state.desired_wrench = massage_wrench + return 2 + if self.pause_envent.is_set(): + self.pause_envent.clear() + self.logger.log_error("self.pause_envent.clear()") + self.arm_state.desired_position = copy.deepcopy(self.arm_state.arm_position) + self.arm_state.desired_orientation = copy.deepcopy(self.arm_state.arm_orientation) + massage_wrench = np.array([0, 0, 0, 0, 0, 0]) + self.arm_state.desired_wrench = massage_wrench + return 5 + if self.skip_envent.is_set(): + self.logger.log_error("self.skip_envent.clear()") + self.arm_state.desired_position = copy.deepcopy(self.arm_state.arm_position) + self.arm_state.desired_orientation = copy.deepcopy(self.arm_state.arm_orientation) + massage_wrench = np.array([0, 0, 0, 0, 0, 0]) + self.arm_state.desired_wrench = massage_wrench + return 6 + if self.arm.is_exit or self.exit_event.is_set(): + print("robot_hardware_error:",self.arm.is_exit,self.exit_event.is_set()) + return 3 + if self.adjust_wrench_envent.is_set() and self.is_execute == True: + if self.adjust_wrench is not None: + self.arm_state.desired_wrench = self.adjust_wrench + self.adjust_wrench_envent.clear() + + self.arm_state.desired_position = pos_traj[i] + self.arm_state.desired_orientation = quat_traj[i] + + if self.ion is not None: + + act_position = R.from_quat(self.arm_state.arm_orientation).as_matrix().T @ self.arm_state.arm_position + desired_position = R.from_quat(self.arm_state.arm_orientation).as_matrix().T @ self.arm_state.desired_position + if self.arm_state.positionerSensorData < 0.001: + desired_position[2] = desired_position[2] - self.arm_state.desired_high + else: + desired_position[2] = min(act_position[2] + self.arm_state.positionerSensorData - self.arm_state.desired_high , desired_position[2] - self.arm_state.desired_high) + self.arm_state.desired_position = R.from_quat(self.arm_state.arm_orientation).as_matrix() @ desired_position + self.command_rate.precise_sleep() + + start = time.time() + + while wait: + if self.interrupt_event.is_set(): + self.interrupt_event.clear() + return 2 + if self.pause_envent.is_set(): + self.pause_envent.clear() + return 5 + if self.skip_envent.is_set(): + # self.skip_envent.clear() + return 6 + if self.arm.is_exit or self.exit_event.is_set(): + print("robot_hardware_error:",self.arm.is_exit,self.exit_event.is_set()) + return 3 + if time.time() - start > timeout: + return 1 + self.command_rate.precise_sleep() + + return 0 + + @track_function("apply_force",True) + def apply_force(self, t, wrench, interpolation:Literal["linear","oscillation"] = "linear", algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance"): + """ + 施加力 + param: + t: 时间或时间序列 + wrench(nx6): 力矩或力矩序列,当输入为None时,表示力为[0,0,0,0,0,0] + is_interrupt: 是否允许中断 + wait: 是否等待运动完成 + timeout: 超时时间 + interpolation: 插值方式 + algorithm: 控制算法 + return: + code: 0表示成功 + 1表示超时 + 2表示用户打断 + 3表示硬件错误 + 4表示参数错误 + """ + + if t==0: + self.arm_state.desired_wrench = wrench + return 0 + + # 在函数内部直接规划力矩轨迹 + traj = self.wrench_traj_generate(t, wrench, interpolation=interpolation) + # # 在函数内部直接规划力矩轨迹 + # traj = self.traj_generate(t, None, wrench, interpolation=interpolation) + if traj is None: + return 4 + + time_seq = traj['t'] + wrench_traj = traj['wrench'] + + # try: + # self.controller_manager.switch_controller(algorithm) + # except ValueError as error: + # self.logger.log_error(error) + # return 4 + + self.logger.log_info("切换到%s控制器" % algorithm) + + for i in range(len(time_seq)): + if self.interrupt_event.is_set(): + self.interrupt_event.clear() + return 2 + if self.pause_envent.is_set(): + self.pause_envent.clear() + return 5 + if self.skip_envent.is_set(): + self.skip_envent.clear() + return 6 + if self.arm.is_exit or self.exit_event.is_set(): + return 3 + if self.adjust_wrench_envent.is_set(): + # if wrench_traj is not None: + # wrench_traj[i:] = self.adjust_wrench + self.adjust_wrench_envent.clear() + # print(wrench_traj[i]) + self.arm_state.desired_wrench = wrench_traj[i] if wrench_traj is not None else np.zeros(6, dtype=np.float64) + self.command_rate.precise_sleep() + + return 0 + + @track_function("resample_curve_strict",True) + def resample_curve_strict(self, points, num_resampled_points): + """ + 修正的曲线重新采样函数，移除重复点并确保累积弧长严格递增。 + """ + # 计算累积弧长 + distances = np.sqrt(np.sum(np.diff(points, axis=0)**2, axis=1)) # 计算相邻点之间的距离 + cumulative_length = np.insert(np.cumsum(distances), 0, 0) # 计算累积弧长 + + # 确保累积弧长严格递增（去除重复值） + unique_indices = np.where(np.diff(cumulative_length, prepend=-np.inf) > 0)[0] + cumulative_length = cumulative_length[unique_indices] + points = points[unique_indices] + + # 生成均匀间隔的新弧长 + target_lengths = np.linspace(0, cumulative_length[-1], num_resampled_points) + + # 在累积弧长上进行插值计算新点位置 + x_interp = interp1d(cumulative_length, points[:, 0], kind='linear', fill_value="extrapolate") + y_interp = interp1d(cumulative_length, points[:, 1], kind='linear', fill_value="extrapolate") + + new_x = x_interp(target_lengths) + new_y = y_interp(target_lengths) + + return np.column_stack((np.round(new_x).astype(int), np.round(new_y).astype(int))) + + @track_function("generate_in_spiral_cloudPoint",True) + def generate_in_spiral_cloudPoint(self, start, end, num_cycle_factor=None, width=None, direction:Literal["CW","CCW"] ='CW'): + """ + 绘制由内向外的螺旋线。 + + 参数: + - start: 起点坐标 (x, y) + - end: 终点坐标 (x, y) + - num_cycle_factor: 螺旋的圈数 + - points: 螺旋线上的点的数量 + """ + # 计算起点和终点之间的距离 + dx = end[0] - start[0] + dy = end[1] - start[1] + distance = np.sqrt(dx**2 + dy**2) + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 5 + + num_cycle_factor = np.round(num_cycle_factor).astype(int) + if distance < 2: + print("距离过短，无法绘制螺旋线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + angle = np.arctan2(dy, dx) # 计算从起点到终点的角度 + + points = np.round(num_cycle_factor *2*np.pi/(np.pi/9)).astype(int) + + # 螺旋的角度范围 + theta = np.linspace(0, 2 * np.pi * num_cycle_factor, points) + + # 螺旋的半径随着角度增大而线性增长 + r = np.linspace(0, distance, points) + + tempK = np.abs(width/(distance*((2*num_cycle_factor-0.75)/num_cycle_factor))) + + # 计算螺旋的x, y坐标 + if direction == "CCW": + # 反向旋转时，使用负的角度来反向螺旋 + x = r * np.cos(-theta) # 逆时针旋转 + y = r * np.sin(-theta)*tempK + else: + # 正常旋转 + x = r * np.cos(theta) + y = r * np.sin(theta)*tempK + + # 旋转螺旋线以对齐起点和终点的角度 + x_rotated = x * np.cos(angle) - y * np.sin(angle) + y_rotated = x * np.sin(angle) + y * np.cos(angle) + + # 平移螺旋线，使其从起点开始 + x_final = x_rotated + start[0] - x_rotated[0] + y_final = y_rotated + start[1] - y_rotated[0] + + # return np.hstack((np.round(x_final).astype(int).reshape(-1, 1), np.round(y_final).astype(int).reshape(-1, 1))) + result_points = np.hstack((np.round(x_final).astype(int).reshape(-1, 1), + np.round(y_final).astype(int).reshape(-1, 1))) + + return self.resample_curve_strict(points=result_points, num_resampled_points=len(result_points)) + + @track_function("generate_out_spiral_cloudPoint",True) + def generate_out_spiral_cloudPoint(self,start, end, num_cycle_factor=None, width=None, direction:Literal["CW","CCW"] ='CW'): + """ + 绘制由外向内的螺旋线。 + + 参数: + - start: 起点坐标 (x, y) + - end: 终点坐标 (x, y) + - num_cycle_factor: 螺旋的圈数 + - points: 螺旋线上的点的数量 + """ + # 计算起点和终点之间的距离 + dx = end[0] - start[0] + dy = end[1] - start[1] + angle = np.arctan2(dy, dx) # 计算从起点到终点的角度 + distance = np.sqrt(dx**2 + dy**2) + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 5 + + num_cycle_factor = np.round(num_cycle_factor).astype(int) + if distance < 2: + print("距离过短，无法绘制螺旋线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + points = np.round(num_cycle_factor *2*np.pi/(np.pi/9)).astype(int) + + # 螺旋的角度范围 + theta = np.linspace(0, 2 * np.pi * num_cycle_factor, points) + + # 螺旋的半径随着角度增大而线性增长 + r = np.linspace(distance, 0, points) + + tempK = np.abs(width/(distance*((2*num_cycle_factor-0.75)/num_cycle_factor))) + + # 计算螺旋的x, y坐标 + if direction == "CCW": + # 反向旋转时，使用负的角度来反向螺旋 + x = r * np.cos(-theta) # 逆时针旋转 + y = r * np.sin(-theta)*tempK + else: + # 正常旋转 + x = r * np.cos(theta) + y = r * np.sin(theta)*tempK + + # 旋转螺旋线以对齐起点和终点的角度 + x = x * np.cos(np.pi) - y * np.sin(np.pi) + y = x * np.sin(np.pi) + y * np.cos(np.pi) + + # 旋转螺旋线以对齐起点和终点的角度 + x_rotated = x * np.cos(angle) - y * np.sin(angle) + y_rotated = x * np.sin(angle) + y * np.cos(angle) + + # 平移螺旋线，使其从起点开始 + x_final = x_rotated + start[0] - x_rotated[0] + y_final = y_rotated + start[1] - y_rotated[0] + + # return np.hstack((np.round(x_final).astype(int).reshape(-1, 1), np.round(y_final).astype(int).reshape(-1, 1))) + result_points = np.hstack((np.round(x_final).astype(int).reshape(-1, 1), + np.round(y_final).astype(int).reshape(-1, 1))) + + return self.resample_curve_strict(points=result_points, num_resampled_points=len(result_points)) + + @track_function("generate_prolateCycloid_cloudPoint",True) + def generate_prolateCycloid_cloudPoint(self, start, end, width=None, num_cycle_factor=None, direction: Literal["CW", "CCW"] = 'CW'): + # Parameters + step_degree = np.pi / 12 + a_prolate = 4.5 + radius = 30.0 + distance = np.linalg.norm(np.array(end) - np.array(start)) + angle = np.arctan2(end[1] - start[1], end[0] - start[0]) + + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 5 + num_cycle_factor = np.round(num_cycle_factor).astype(int) + if distance < 2: + print("距离过短，无法绘制Cycloid线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + r_cycloid = radius / (2 * a_prolate) + num_cycles = int(num_cycle_factor) + temp_k = distance / (r_cycloid * (np.pi + 2 * a_prolate + 2 * np.pi * num_cycles)) + + # Generate the prolate cycloid + theta_prolate = np.linspace(0.5 * np.pi, num_cycles * 2 * np.pi + 1.5 * np.pi, + round((num_cycles * 2 * np.pi + 1.5 * np.pi) / step_degree)) + if direction == 'CW': + theta_prolate = np.flip(theta_prolate) + + x_prolate = r_cycloid * (theta_prolate - a_prolate * np.sin(theta_prolate)) * temp_k + y_prolate = r_cycloid * (1 - a_prolate * np.cos(theta_prolate)) + + # Scale y-prolate based on width + y_prolate = (y_prolate / 30) * width + + # Rotation of cycloid + if direction == 'CW': + x_prolate, y_prolate = -x_prolate, -y_prolate + + x_rotated = x_prolate * np.cos(angle) - y_prolate * np.sin(angle) + y_rotated = x_prolate * np.sin(angle) + y_prolate * np.cos(angle) + + # Translate to starting point + x_final = x_rotated + start[0] - x_rotated[0] + y_final = y_rotated + start[1] - y_rotated[0] + + return np.column_stack((np.round(x_final).astype(int), np.round(y_final).astype(int))) + + @track_function("generate_ellipse_cloudPoint",True) + def generate_ellipse_cloudPoint(self,start, end, num_cycle_factor=None, width=None, direction:Literal["CW","CCW"] ='CW'): + # 计算椭圆的中心（两个端点的中点） + center = (start + end) / 2 + if num_cycle_factor is None: + num_cycle_factor = 1 + + num_cycle_factor = np.round(num_cycle_factor).astype(int) + + distance = np.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2) + + if width is None: + width = distance/2 + + + if distance < 2: + print("距离过短，无法绘制椭圆") + return 0 + if width <= 2: + print("椭圆半径过小，无法绘制椭圆") + width = self.width_default + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + # 计算两个端点的距离，即长半轴长度 + a = np.sqrt((end[0] - start[0])**2 + (end[1] - start[1])**2) / 2 + + # 假设短半轴长度为常量值radius + b = width / 2 + + # 计算旋转角度（由端点的方向确定） + angle = np.arctan2(end[1] - start[1], end[0] - start[0]) + points = np.round(((num_cycle_factor + 1) * np.pi * 2)/(np.pi/9)).astype(int) + + # 绘制椭圆的参数化公式 + theta = np.linspace(np.pi, (2+num_cycle_factor*2)* np.pi, points) + + # 计算螺旋的x, y坐标 + if direction == "CCW": + # 反向旋转时，使用负的角度来反向螺旋 + x_ellipse = a * np.cos(-theta) #顺时针 + y_ellipse = b * np.sin(-theta) + else: + # 正常旋转 + x_ellipse = a * np.cos(theta) + y_ellipse = b * np.sin(theta) + + # 绕中心旋转椭圆 + x_rot = center[0] + x_ellipse * np.cos(angle) - y_ellipse * np.sin(angle) + y_rot = center[1] + x_ellipse * np.sin(angle) + y_ellipse * np.cos(angle) + + # 返回椭圆点云，保留整数值 + return np.hstack((np.round(x_rot).astype(int).reshape(-1, 1), np.round(y_rot).astype(int).reshape(-1, 1))) + + @track_function("generate_Lemniscate_cloudPoint",True) + def generate_Lemniscate_cloudPoint(self, start, end, width = None,num_cycle_factor = None, direction:Literal["CW","CCW"] ='CW'): + """ + 生成从start到end的8字形插补路径 + + :param start: 起点坐标 (x0, y0) + :param end: 终点坐标 (x1, y1) + :param num_points: 插补点的数量 + :return: 插补路径的坐标列表 + """ + # 起点和终点的坐标 + x1, y1 = start + x0, y0 = end + step_degree=np.pi/12 + + # 计算轨迹的缩放因子和旋转角度 + distance = np.linalg.norm([x1 - x0, y1 - y0]) + + if width is None: + width = self.width_default + if num_cycle_factor is None: + num_cycle_factor = 1 + if distance < 2: + print("距离过短，无法绘制螺旋线") + return 0 + if width <= 2: + width = 2 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + + a = distance / 2 # 轨迹的宽度一半 + b = width/2 + angle = np.arctan2(y1 - y0, x1 - x0) + + num_points = np.abs((2*num_cycle_factor+1)*np.pi/step_degree).astype(int) + + # 生成标准的8字形轨迹 (Gerono卵形线) + t_values = np.linspace(0.5*np.pi, (2*num_cycle_factor + 1.5)* np.pi, num_points) + + x_curve = a * np.sin(t_values) + if direction == 'CCW': + y_curve = -b * np.sin(2 * t_values) + else: + y_curve = b * np.sin(2 * t_values) + + # 构建旋转矩阵 + rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)], + [np.sin(angle), np.cos(angle)]]) + + # 旋转并平移轨迹 + curve_points = np.vstack((x_curve, y_curve)).T + rotated_points = curve_points @ rotation_matrix.T + shifted_points = rotated_points + [(x0 + x1) / 2, (y0 + y1) / 2] + + # 将插值结果四舍五入为整数 + return np.round(shifted_points).astype(int) + + @track_function("generate_circle_cloudPoint",True) + def generate_circle_cloudPoint(self, start_point, center, step_degree=np.pi/12, num_cycle_factor = 3): + """ + center: 圆心坐标，形如 (x_c, y_c) + start_point: 起始点坐标，形如 (x_0, y_0) + radius: 圆的半径 + delta_theta: 每次插补的角度增量 + num_turns: 绕圈的次数 + """ + # 确定总共需要生成的插补点数 + num_points = int((2 * np.pi * num_cycle_factor) / step_degree) + + # 圆心 + x_c, y_c = center + + radius = np.linalg.norm(np.array(start_point) - np.array(center)) # 半径 + + # 计算起始点的初始角度 + x_0, y_0 = start_point + theta_0 = np.arctan2(y_0 - y_c, x_0 - x_c) + + # 初始化存储插补点的列表 + circle_points = [] + + # 生成插补点 + for i in range(num_points): + # 当前角度 + theta_i = theta_0 + i * step_degree + + # 计算插补点的坐标 + x_i = x_c + radius * np.cos(theta_i) + y_i = y_c + radius * np.sin(theta_i) + + # 将点添加到列表中 + + circle_points.append((np.round(x_i).astype(int), np.round(y_i).astype(int))) + + circle_points.append((np.round(x_0).astype(int), np.round(y_0).astype(int))) + + return circle_points + + @track_function("generate_point_cloudPoint",True) + def generate_point_cloudPoint(self, point, num_cycle_factor=None, width=None, direction: Literal["CW", "CCW"] = 'CW'): + if num_cycle_factor is None: + num_cycle_factor = 12 + if num_cycle_factor < 1: + print("圈数必须为大于1的整数") + num_cycle_factor = 1 + num_cycle_factor = np.abs(np.round(num_cycle_factor).astype(int)) + if width is None: + width = 20 + if width < 2: + print("宽度必须为大于2的整数") + width = 2 + width = np.abs(np.round(width).astype(int)) + + radius = width/2 + + points = np.round((num_cycle_factor * np.pi * 2)/(np.pi/9)).astype(int) + thetas = np.linspace(0, num_cycle_factor * 2 * np.pi, points) + radiuss = np.ones_like(thetas) * radius + + for i in range(len(thetas)): + if thetas[i] <= np.pi/2: + radiuss[i] = np.sin(thetas[i]) * radius + if thetas[i] >= num_cycle_factor * np.pi * 2 - np.pi / 2: + radiuss[i] = np.sin(num_cycle_factor * np.pi * 2 - thetas[i]) * radius + + + + if direction == "CCW": + x_ellipse = radiuss * np.cos(-thetas) + y_ellipse = radiuss * np.sin(-thetas) + else: + x_ellipse = radiuss * np.cos(thetas) + y_ellipse = radiuss * np.sin(thetas) + x_rot = point[0] + x_ellipse + y_rot = point[1] + y_ellipse + result_points = np.hstack((np.round(x_rot).astype(int).reshape(-1, 1), np.round(y_rot).astype(int).reshape(-1, 1))) + return self.resample_curve_strict(points=result_points, num_resampled_points=len(result_points)) + + @track_function("generate_line_cloudPoint",True) + def generate_line_cloudPoint(self,start, end, step_distance = 5): + """ + 生成起点和终点之间的线性插值点 + + 参数: + start: 起点 (标量或数组，例如 [x1, y1, z1]) + end: 终点 (标量或数组，例如 [x2, y2, z2]) + num_points: 插值点的数量（包括起点和终点） + + 返回: + 包含线性插值点的数组 + """ + start = np.array(start) + end = np.array(end) + distance = np.linalg.norm(end - start) + num_points = int(distance / step_distance) + + if num_points <=2: + num_points = 2 + # 使用 numpy 的 linspace 在起点和终点之间生成线性插值点 + interpolated_points = np.linspace(start, end, num_points) + + # 将插值结果四舍五入为整数 + return np.round(interpolated_points).astype(int) + + @track_function("generate_repeat_line_cloudPoint",True) + def generate_repeat_line_cloudPoint(self, start_point, end_point, num_cycle_factor = 5): + """ + start_point: 起始点坐标，形如 (x_0, y_0) + end_point: 终点坐标，形如 (x_0, y_0) + num_cycle_factor: 重复的次数 + """ + + # 初始化存储插补点的列表 + result_points = [] + # 生成插补点 + for i in range(num_cycle_factor): + # 将点添加到列表中 + result_points.append(start_point) + result_points.append(end_point) + return np.round(np.array(result_points)).astype(int) + + @track_function("generate_finger_circle_cloudPoint",True) + def generate_finger_circle_cloudPoint(self, center_point, num_cycle_factor=5, radius=30,deviation=15): + """ + 生成一指禅绕球心的圆弧轨迹 + center_point: 圆心坐标 [x, y, z, roll, pitch, yaw] + num_cycle_factor: 轨迹的重复次数 + radius: 圆的半径 + """ + result_points = [] + result_points.append(copy.deepcopy(center_point)) + deviation = deviation * np.pi/180 + + totalCounts = int(360 / radius) + shift = 1 + + + for i in range(num_cycle_factor): + for countR in range(0, totalCounts): + # 当前旋转矩阵 + temp_pose = copy.deepcopy(center_point) + rotation = R.from_euler('xyz', temp_pose[3:], degrees=False) + current_rotation_matrix = rotation.as_matrix() + + + if i == 0: + shift = countR/(90/radius) if countR/(90/radius) < 1 else 1 + if i == num_cycle_factor-1: + shift = (totalCounts-countR)/(90/radius) if (totalCounts-countR)/(90/radius) < 1 else 1 + + + # 生成增量旋转矩阵 + increment_rotation= R.from_euler('xyz', [deviation*shift,0,countR * np.pi*2 /totalCounts], degrees=False).as_matrix() + + # 更新旋转矩阵 + updated_rotation_matrix = current_rotation_matrix @ increment_rotation + + # 将旋转矩阵转换回欧拉角 + temp_pose[3:] = R.from_matrix(updated_rotation_matrix).as_euler('xyz', degrees=False) + + # 添加当前点到结果列表 + result_points.append(copy.deepcopy(temp_pose)) + + result_points.append(copy.deepcopy(center_point)) + return result_points + + def traj_generate(self, t: Union[int, float, List[float]], pose = None, wrench = None, interpolation: Literal["linear", "cubic","circle","cloud_point"] = 'linear',**kwargs): + """ + 轨迹生成 + param: + t: 时间或时间序列 + pose(nx6): 世界坐标系下的位姿或位姿序列,当输入为None时,表示保持当前位姿 + wrench(nx6): 力矩或力矩序列,当输入为None时,表示保持当前的力 + interpolation: 插值方式 + return: + traj: 生成的轨迹 -> dict + """ + # 确保时间输入是列表 + if isinstance(t, (int, float)): + t = [t] + elif not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t): + self.logger.log_error("not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t)") + return None + # 检查时间点数量 + if len(t) < 1: + self.logger.log_error("len(t) < 1") + return None + # 时间序列 + time_points = [0.0] + t + time_points = np.array(time_points) + dt = self.command_rate.to_sec() + + for i in range(len(time_points)): + time_points[i] = np.round(time_points[i] / dt) * dt + if np.abs(time_points[-1]) < dt: + time_points[-1] = dt + # 由于浮点数精度问题，time_step要乘0.9 + time_seq = np.arange(time_points[0], time_points[-1] + dt * 0.9, dt) + pos_traj = None + quat_traj = None + wrench_traj = None + pos_vel = None + angular_vel = None + # 位置轨迹与速度轨迹生成 + if pose is not None: + if interpolation != 'cloud_point': + pose = np.array(pose).reshape(-1, 6) + if pose.shape[0] != len(t): + self.logger.log_error("pose.shape[0] != len(t)") + print(pose.shape[0],len(t)) + return None + pose_matrix = self.convert_to_7d(pose) + else: + pose = np.array(pose) + try: + # 插值 + if interpolation == 'linear': + pos_traj, quat_traj = linear_interpolate(positions=pose_matrix[:, :3], quaternions=pose_matrix[:, 3:], time_points=time_points, time_step=dt) + elif interpolation == 'cubic': + pos_traj, quat_traj = spline_interpolate(positions=pose_matrix[:, :3], quaternions=pose_matrix[:, 3:], time_points=time_points, time_step=dt) + elif interpolation == 'circle': + if self.ion is not None: + pos_traj = circle_trajectory(center=pose, radius=0.07,time_points=time_points,time_step=dt,**kwargs) + else: + pos_traj = circle_trajectory(center=pose,time_points=time_points,time_step=dt,**kwargs) + quat_traj = np.tile(R.from_euler('xyz', np.array(pose[0][3:])).as_quat(), (pos_traj.shape[0], 1)) + elif interpolation == 'cloud_point': + pos_traj, quat_traj = cloud_point_interpolate(positions=pose[:, :3], quaternions=pose[:, 3:], time_points=time_points, time_step=dt) + # self.logger.log_yellow(f'{pos_traj[0]} {pos_traj[-1]}') + except ValueError as error: + self.logger.log_error(error) + return None + else: + self.logger.log_error("未输入位置进行规划") + + # TODO 力轨迹生成 改为线性增加 + if wrench is not None: + wrench = np.array(wrench).reshape(-1, 6) + if wrench.shape[0] != len(t): + return None + indices = np.searchsorted(time_points, time_seq, side='left') - 1 + indices = np.clip(indices, 0, len(wrench) - 1) + wrench_traj = wrench[indices] + # 构建返回数据字典 + traj = { + 't': time_seq, + 'pos': pos_traj, + 'quat': quat_traj, + 'pos_vel': pos_vel, + 'angular_vel': angular_vel, + 'wrench': wrench_traj + } + return traj + + + def wrench_traj_generate(self, t: Union[int, float, List[float]], wrench = None, interpolation: Literal["linear", "oscillation"] = 'linear',**kwargs): + """ + 力/力矩轨迹生成 + param: + t: 时间或时间序列 + wrench(nx6): 力矩或力矩序列 + interpolation: 插值方式 + return: + traj: 生成的轨迹 -> dict + """ + # 确保时间输入是列表 + if isinstance(t, (int, float)): + t = [t] + elif not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t): + self.logger.log_error("not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t)") + return None + # 检查时间点数量 + if len(t) < 1: + self.logger.log_error("len(t) < 1") + return None + + if wrench is None: + self.logger.log_error("未输入力矩进行规划") + return None + # 时间序列 + time_points = [0.0] + t + time_points = np.array(time_points) + dt = self.command_rate.to_sec() + + for i in range(len(time_points)): + time_points[i] = np.round(time_points[i] / dt) * dt + if np.abs(time_points[-1]) < dt: + time_points[-1] = dt + # 由于浮点数精度问题，time_step要乘0.9 + time_seq = np.arange(time_points[0], time_points[-1] + dt * 0.9, dt) + + wrench_traj = None + + + + + # TODO 力轨迹生成 改为线性增加 + if wrench is not None: + wrench = np.array(wrench).reshape(-1, 6) + + + if wrench.shape[0] != len(t): + return None + if interpolation == 'oscillation': + wrench_traj = oscillation_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + else: + wrench_traj = linear_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + + # indices = np.searchsorted(time_points, time_seq, side='left') - 1 + + + # indices = np.clip(indices, 0, len(wrench) - 1) + + # wrench_traj = wrench[indices] + + + # 构建返回数据字典 + traj = { + 't': time_seq, + 'wrench': wrench_traj + } + return traj + + def force_traj_generate(self, t: Union[int, float, List[float]], wrench = None, interpolation: Literal["linear", "oscillation", "auto"] = "auto",**kwargs): + """ + 力/力矩轨迹生成 + param: + t: 时间或时间序列 + wrench(nx6): 力矩或力矩序列 + interpolation: 插值方式 + return: + traj: 生成的轨迹 -> dict + """ + # 确保时间输入是列表 + if isinstance(t, (int, float)): + t = [t] + elif not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t): + self.logger.log_error("not isinstance(t, list) or any(not isinstance(i, (int, float)) for i in t)") + return None + # 检查时间点数量 + if len(t) < 1: + self.logger.log_error("len(t) < 1") + return None + + if wrench is None: + self.logger.log_error("未输入力矩进行规划") + return None + # 时间序列 + time_points = [0.0] + t + time_points = np.array(time_points) + dt = self.command_rate.to_sec() + + for i in range(len(time_points)): + time_points[i] = np.round(time_points[i] / dt) * dt + if np.abs(time_points[-1]) < dt: + time_points[-1] = dt + # 由于浮点数精度问题，time_step要乘0.9 + time_seq = np.arange(time_points[0], time_points[-1] + dt * 0.9, dt) + + wrench_traj = None + + + + + # TODO 力轨迹生成 改为线性增加 + if wrench is not None: + wrench = np.array(wrench).reshape(-1, 6) + + + if wrench.shape[0] != len(t): + return None + if interpolation == 'oscillation': + wrench_traj = self.force_plan.oscillation_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + elif interpolation == "auto": + # target_wrench = [] + # target_wrench.append(wrench) + wrench_traj = self.force_plan.S_shaped_wrench_interpolate(start=self.arm_state.desired_wrench, wrench=wrench, time_points=time_points, time_step=dt) + else: + wrench_traj = self.force_plan.linear_wrench_interpolate(wrench=wrench, time_points=time_points, time_step=dt) + + # indices = np.searchsorted(time_points, time_seq, side='left') - 1 + + + # indices = np.clip(indices, 0, len(wrench) - 1) + + # wrench_traj = wrench[indices] + + + # 构建返回数据字典 + traj = { + 't': time_seq, + 'wrench': wrench_traj + } + return traj + + if __name__ == "__main__": waypoints = [ diff --git a/Massage/MassageControl/algorithms/arm_state.py b/Massage/MassageControl/algorithms/arm_state.py index bdcf7e8..4fd4147 100644 --- a/Massage/MassageControl/algorithms/arm_state.py +++ b/Massage/MassageControl/algorithms/arm_state.py @@ -14,6 +14,10 @@ class ArmState: self.last_arm_orientation = np.array([0.0,0.0,0.0,1.0]) # [qx, qy, qz, qw] self.last_external_wrench_base = np.zeros(6,dtype=np.float64) self.last_external_wrench_tcp = np.zeros(6,dtype=np.float64) + + # 传感器数据 + self.positionerSensorData = 0.0 + self.desired_high = 0.06 # 目标状态 self.desired_position = np.zeros(3,dtype=np.float64) diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index acca5c1..236ef1f 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -5,6 +5,7 @@ except: import threading from time import sleep,time import re +from tools.log import CustomLogger import copy import numpy as np from scipy.spatial.transform import Rotation as R @@ -35,8 +36,8 @@ import math ''' class dobot_nova5: - def __init__(self,ip): - self.ip = ip + def __init__(self,arm_ip = '192.168.5.1'): + self.ip = arm_ip ## 机械臂IP地址 self.dashboardPort = 29999 ## 一发一收-设置-控制及运动端口号 self.feedFourPort = 30004 ## 第四实时反馈(8ms)反馈数据端口号 self.dashboard = None ## 初始化控制及运动端口实例 @@ -60,6 +61,8 @@ class dobot_nova5: self.ToolValue = -1 ''' self.feedbackData = feedbackItem() # 自定义反馈数据结构体 + self.logger = CustomLogger('Dobot_nova5',True) + def parseResultId(self,valueRecv): # 解析返回值 @@ -170,12 +173,14 @@ class dobot_nova5: self.dashboard.Stop() self.dashboard.EmergencyStop(mode=1) print("当前工作停止，机械臂处于急停状态") + return -1 if self.feedbackData.robotMode == 10: # 10 当前工程暂停 print("机械臂为暂停状态") res = self.dashboard.Continue() code = self.parseResultId(res)[0] if code == 0: print("机械臂继续已暂停的运动指令") + return -1 if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID: print("该次运动完成") break diff --git a/Massage/MassageControl/tools/decorator_tools.py b/Massage/MassageControl/tools/decorator_tools.py new file mode 100755 index 0000000..da0ac03 --- /dev/null +++ b/Massage/MassageControl/tools/decorator_tools.py @@ -0,0 +1,29 @@ +from functools import wraps + +def custom_decorator(before=None, after=None): + """ + 创建一个自定义装饰器，可以在函数调用之前和之后执行自定义操作。 + + :param before: 在函数调用之前执行的操作，接受同样的参数。 + :param after: 在函数调用之后执行的操作，接受同样的参数和返回值。 + :return: 一个装饰器。 + """ + def decorator(func): + @wraps(func) + def wrapper(self, *args, **kwargs): + if before: + before(self, *args, **kwargs) + result = func(self, *args, **kwargs) + if after: + after(self, result, *args, **kwargs) + return result + return wrapper + return decorator + +def apply_decorators(cls): + for attr_name, attr_value in cls.__dict__.items(): + if callable(attr_value) and hasattr(attr_value, '_decorator'): + decorated_func = attr_value._decorator(attr_value) + setattr(cls, attr_name, decorated_func) + return cls + diff --git a/Massage/MassageControl/tools/serial_tools.py b/Massage/MassageControl/tools/serial_tools.py new file mode 100755 index 0000000..f25c484 --- /dev/null +++ b/Massage/MassageControl/tools/serial_tools.py @@ -0,0 +1,94 @@ +import serial.tools.list_ports as list_ports +import subprocess + +def find_serial_by_serial_number(serial_number): + ports = list_ports.comports() + for port in ports: + if port.serial_number == serial_number: + return port.device + return None + +def find_serial_by_location(location): + ports = list_ports.comports() + for port in ports: + if port.location == location: + return port.device + return None + +def list_usb_ports_with_details(): + ports = list_ports.comports() + usb_ports = {} + + for port in ports: + if "ttyUSB" in port.device: + usb_ports[port.device] = { + 'serial_number': port.serial_number, + 'vid': port.vid, + 'pid': port.pid, + 'location': port.location, + 'description': port.description, + 'manufacturer': port.manufacturer + } + + return usb_ports + +# 维护 socat 进程的字典，用于跟踪每个虚拟串口的进程 +socat_processes = {} + +def start_virtual_serial(remote_ip, remote_port, device_name): + """ + 启动一个 socat 进程来创建虚拟串口。 + :param remote_ip: 远程主机 IP 地址 + :param remote_port: 远程主机端口 + :param device_name: 虚拟串口的设备名称 + """ + # 构建 socat 命令 + socat_command = f'socat PTY,raw,echo=0,link={device_name} TCP:{remote_ip}:{remote_port}' + # 启动 socat 进程 + process = subprocess.Popen(socat_command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + # 将进程存储到字典中 + socat_processes[device_name] = process + print(f"Started socat process for {device_name} on {remote_ip}:{remote_port}") + +def stop_virtual_serial(device_name): + """ + 停止一个 socat 进程。 + :param device_name: 虚拟串口的设备名称 + """ + # 检查进程是否在字典中 + if device_name in socat_processes: + # 获取进程对象 + process = socat_processes[device_name] + # 使用 pkill 终止相应的 socat 进程 + subprocess.call(['pkill', '-f', f'{device_name}']) + # 终止进程 + process.terminate() + process.wait() + # 移除已终止的进程 + del socat_processes[device_name] + print(f"Stopped socat process for {device_name}") + else: + print(f"No running socat process found for {device_name}") + +if __name__ == "__main__": + usb_ports_with_details = list_usb_ports_with_details() + + if usb_ports_with_details: + print("USB Ports and their Details:") + for device, details in usb_ports_with_details.items(): + print(f"Device: {device}, Serial Number: {details['serial_number']}, VID: {details['vid']}, PID: {details['pid']}, Location: {details['location']}, Description: {details['description']}, Manufacturer: {details['manufacturer']}") + else: + print("No USB ports found.") + + # serial_number = "1234567890" # Replace with your serial number + # port = find_serial_by_serial_number(serial_number) + # if port: + # print(f"Serial number {serial_number} found on port {port}") + # else: + # print(f"Serial number {serial_number} not found") + + port = find_serial_by_location("1-8") + if port: + print(f"Port found: {port}") + else: + print("Port not found") \ No newline at end of file