from collections import deque from scipy.interpolate import interp1d import rospy import rosgraph import smach import smach_ros import sys import psutil from pathlib import Path import websockets import threading import argparse import asyncio import numpy as np import time import json import requests import signal import copy import cv2 import os import shutil import random import yaml import requests from tools.yaml_operator import read_yaml from tools.log import CustomLogger from typing import Literal from MassageControl.MassageRobot_nova5 import MassageRobot from aucpuncture2point import ToolCamera, AbdomenDetector,BackDetector,LegDetector, CoordinateTransformer from scipy.spatial.transform import Rotation as R from scipy.interpolate import make_interp_spline import matplotlib.pyplot as plt import datetime import shutil import importlib.util from MassageControl.hardware.force_sensor import XjcSensor sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from VortXDB.client import VTXClient from functools import wraps # 加个wraps，保持函数名不会丢 # 自定义输出类，将输出同时发送到终端和日志文件 class MultiWriter: def __init__(self, *writers): self.writers = writers def write(self, message): for writer in self.writers: writer.write(message) writer.flush() # 确保及时输出 def flush(self): for writer in self.writers: writer.flush() # 重定向标准输出和标准错误到文件和终端 def redirect_output(log_file): # 保存当前的标准输出和标准错误（终端输出） original_stdout_fd = sys.stdout.fileno() original_stderr_fd = sys.stderr.fileno() # 打开日志文件用于写入 log_fd = os.open(log_file, os.O_WRONLY | os.O_CREAT | os.O_TRUNC) # 保留原始的标准输出和错误输出，创建文件对象 original_stdout = os.fdopen(os.dup(original_stdout_fd), 'w') original_stderr = os.fdopen(os.dup(original_stderr_fd), 'w') # 将 stdout 和 stderr 分别指向日志文件 os.dup2(log_fd, original_stdout_fd) os.dup2(log_fd, original_stderr_fd) # 将输出同时发送到终端和日志文件 sys.stdout = MultiWriter(original_stdout, os.fdopen(log_fd, 'w')) sys.stderr = MultiWriter(original_stderr, os.fdopen(log_fd, 'w')) timestamp = datetime.datetime.now().strftime("%Y-%m-%d_%H%M%S") # 目标目录和目标文件路径（将日志文件复制到新的位置） target_dir = '/home/jsfb/jsfb_ws/MassageLog/' os.makedirs(target_dir, exist_ok=True) # 确保目标目录存在 source_log_file = '../log/Massage.log' # 如果源文件不存在，则创建一个空文件 if not os.path.exists(source_log_file): with open(source_log_file, 'w'): pass # 创建空文件 # 定义复制的目标文件路径（带时间戳） target_file = os.path.join(target_dir, f'Massage_{timestamp}.log') # 复制文件到新位置 shutil.copy('../log/Massage.log', target_file) log_file = '../log/Massage.log' redirect_output(log_file) # 定义模块加载的函数 def load_class_from_pyc(pyc_path, class_name, module_name): """ 动态加载 .pyc 文件并尝试访问其中的类。 :param pyc_path: .pyc 文件路径 :param class_name: 需要访问的类名 :param module_name: 模块的名称（用于调试和错误信息） :return: 类对象（如果加载和访问成功） :raises: 文件未找到或类未定义等异常 """ # 检查文件是否存在 if not os.path.exists(pyc_path): raise FileNotFoundError(f"Module file '{pyc_path}' not found.") try: # 动态加载模块 spec = importlib.util.spec_from_file_location(module_name, pyc_path) module = importlib.util.module_from_spec(spec) sys.modules[module_name] = module spec.loader.exec_module(module) # 尝试获取指定的类 class_obj = getattr(module, class_name, None) if class_obj is None: raise AttributeError(f"Class '{class_name}' not found in module '{module_name}'.") return class_obj # 返回类对象 except Exception as e: raise RuntimeError(f"Failed to load class '{class_name}' from module '{module_name}': {str(e)}") # 定义 .pyc 文件的路径 shockwave_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/shockwave.pyc' thermotherapy_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/thermotherapy.pyc' stone_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/stone.pyc' ion_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/ion.pyc' heat_pyc_path = '/home/jsfb/jsfb_ws/hardware_bin/heat.pyc' # 使用封装的函数加载各个模块和类 try: Shockwave = load_class_from_pyc(shockwave_pyc_path, "Shockwave", "shockwave") print("Shockwave class loaded successfully.") except Exception as e: print(f"Error loading Shockwave: {str(e)}") try: Thermotherapy = load_class_from_pyc(thermotherapy_pyc_path, "Thermotherapy", "thermotherapy") print("Thermotherapy class loaded successfully.") except Exception as e: print(f"Error loading Thermotherapy: {str(e)}") try: Stone = load_class_from_pyc(stone_pyc_path, "Stone", "stone") print("Stone class loaded successfully.") except Exception as e: print(f"Error loading Stone: {str(e)}") try: Ion = load_class_from_pyc(ion_pyc_path, "Ion", "ion") print("Ion class loaded successfully.") except Exception as e: print(f"Error loading Ion: {str(e)}") try: Heat = load_class_from_pyc(heat_pyc_path, "Heat", "heat") print("Heat class loaded successfully.") except Exception as e: print(f"Error loading Heat: {str(e)}") def monitor_cpu_usage(func): @wraps(func) def wrapper(*args, **kwargs): process = psutil.Process(os.getpid()) # 获取执行前的 I/O 数据 io_start = process.io_counters() # 记录执行前的内存使用情况 memory_start = process.memory_info().rss / 1024 / 1024 # 以 MB 为单位 # 记录执行前的 CPU 占用百分比 cpu_start_percent = process.cpu_percent(interval=None) cpu_start = process.cpu_times() time_start = time.time() result = func(*args, **kwargs) time_end = time.time() cpu_end = process.cpu_times() # 获取执行后的 I/O 数据 io_end = process.io_counters() # 记录执行后的内存使用情况 memory_end = process.memory_info().rss / 1024 / 1024 # 以 MB 为单位 # 记录执行后的 CPU 占用百分比 cpu_end_percent = process.cpu_percent(interval=None) # 计算 I/O 相关的差值 read_bytes = io_end.read_bytes - io_start.read_bytes write_bytes = io_end.write_bytes - io_start.write_bytes user_time = cpu_end.user - cpu_start.user system_time = cpu_end.system - cpu_start.system wall_time = time_end - time_start # 打印 CPU 占用情况 print(f"\n【函数 {func.__name__} 执行完毕】") print(f" CPU用户态时间: {user_time:.4f}秒") print(f" CPU内核态时间: {system_time:.4f}秒") print(f" 总墙钟时间(实际时间): {wall_time:.4f}秒") print(f" 执行前 CPU 占用百分比: {cpu_start_percent:.2f}%") print(f" 执行后 CPU 占用百分比: {cpu_end_percent:.2f}%") print(f" 执行前内存使用: {memory_start:.2f}MB") print(f" 执行后内存使用: {memory_end:.2f}MB") print(f" 内存消耗: {memory_end - memory_start:.2f}MB") print(f" 读取字节数: {read_bytes / 1024:.2f}KB") print(f" 写入字节数: {write_bytes / 1024:.2f}KB") return result return wrapper def parse_controller_args(): parser = argparse.ArgumentParser(description="Admittance control") parser.add_argument( "--massagecontrol_path", type=str, default="MassageControl/config/robot_config.yaml", ) parser.add_argument("--force_limit", type=list, default=[-70, -5]) parser.add_argument("--unit_length", type=float, default=0.07) parser.add_argument("--temperature_limit", type=list, default=[0, 5]) parser.add_argument("--speed_limit", type=list, default=[0, 5]) parser.add_argument("--gear_limit", type=list, default=[0, 5]) parser.add_argument("--shake_limit", type=list, default=[0, 5]) parser.add_argument("--press_limit", type=list, default=[1, 27]) parser.add_argument("--frequency_limit", type=list, default=[1, 16]) parser.add_argument("--high_limit", type=list, default=[0.04, 0.15]) args = parser.parse_args() return args class StopWorkflow(Exception): pass class PauseWorkflow(Exception): pass class EmergencyStopWorkflow(Exception): pass class SharedResources: def __init__(self, args): # 日志 self.logger = CustomLogger('Massage',True) self.vtxdb = VTXClient(False) # 机械臂对象 self.robot = MassageRobot(args.massagecontrol_path) position,quat_rot = self.robot.arm.get_arm_position() euler_rot = R.from_quat(quat_rot).as_euler('xyz') self.ready_pose = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]] self.robot.init_hardwares(self.ready_pose) time.sleep(1) # 事件 self.begin_event = threading.Event() self.stop_event = threading.Event() self.pause_event = threading.Event() self.skip_event = threading.Event() self.adjust_event = threading.Event() self.high_st_limit = 0.95 self.low_st_limit = 0.5 # self.cam_length = 1.70 # 1.80 # 变量 self.choose_task = None self.position_increment = np.array([0, 0, 0]) self.now_state = "IDLE" self.current_part = None # 正在运行的部位 self.task_time = 0 self.current_time = 0 self.loops = 0 self.body_part = None self.progress = 0 # 按摩进度 self.robot.arm_state.massage_wrench = np.array([0, 0, -20, 0, 0, 0]) self.temperature = 3 self.gear = 0 self.shake = 0 self.press = 12 self.frequency = 6 self.speed = 2 self.direction = 1 # self.is_execute = False self.temper_head = None self.temperature_default = 3 #热疗头温度档位默认值 self.temperature_default_thermotherapy = 3 #热疗头温度档位默认值 self.temperature_default_stone = 1 #砭石头温度档位默认参数 self.temperature_default_ion = 1 self.temperature_default_heat = 1 #能量热疗温度档位默认参数 self.gear_default = self.gear self.shake_default = self.shake self.press_default = self.press self.frequency_default = self.frequency self.speed_default = self.speed self.direction_default = self.direction self.desired_high_default = 0.06 self.cloud_points = np.array([[1,1]]) self.cloud_points_shoulder = np.array([[1,1]]) self.abdomen_detector = AbdomenDetector() self.back_detector = BackDetector() self.leg_detector = LegDetector() self.coordinate_tf = None self.acupoint_key_data = None self.y_median_depth_list =None self.x_avg = None self.intrinsics = None # 常量 self.unit_length = args.unit_length self.force_limit = args.force_limit self.temperature_limit = args.temperature_limit self.speed_limit = args.speed_limit self.gear_limit = args.gear_limit self.shake_limit = args.shake_limit self.press_limit = args.press_limit self.frequency_limit = args.frequency_limit self.high_limit = args.high_limit # 事件循环 self.loop = asyncio.new_event_loop() # 相机 self.tool_cam = None self.license_check = False self.is_acupoint = False self.mode_real = 1 #是否开启拍照,测试时设为0 self.is_switch_controller = True self.use_algorithm = "hybrid" self.broken_test = 0 self.show_image_num = 0 self.next_points = None self.mode_commands_list = deque([]) self.mode_commands_list_default = deque([]) self.move_count = 0 self.use_mode = "smart" self.is_pause = False self.is_finish = False self.is_insert = False self.is_skip = False self.path_length = 0 self.path_velocity = 30 self.is_first_move = True self.is_jump = False self.jump_length = 0 self.jump_velocity = 60 self.is_acupoint = False self.plan_task = None self.motion_task = None self.generate_thread = None self.insert_queue_thread = None self.current_queue_num = -2 self.massage_task_num = 0 self.insert_len = 0 self.motion_count = 0 self.start_time = None self.jump_mode = True self.jump_mode_default = True self.is_headOn = True self.is_headOn_default = True self.cal_acu = False self.get_picture = True self.manual_start = False self.show_picture_type = 1 self.manual_stage = 0 self.acupuncture_dict = None self.selected_plan = None self.target_points = None self.update_thread = None self.update_massageHead_temper_thread = None async def client(self, instruction, uri="ws://localhost:8766"): async with websockets.connect(uri) as websocket: self.logger.log_info("begin to send") await websocket.send(instruction) self.logger.log_info("Message " + instruction + " sent to the server") # 接收来自服务器的响应 response = await websocket.recv() print(f"Received response: {response}") def send_instruction(self, instruction, target="language"): if isinstance(instruction, dict) and "progress" in instruction: if int(instruction["progress"]) > self.progress: self.progress = int(instruction["progress"]) try: if target == "language": uri = "ws://localhost:8766" asyncio.run_coroutine_threadsafe( self.client(instruction, uri), self.loop ) elif target == "ui_update_massage_status": requests.post( "http://127.0.0.1:5000/update_massage_status", data=instruction ) self.logger.log_info( "Instruction " + str(instruction) + " sent to ui server" ) elif target == "ui_change_image": requests.post( "http://127.0.0.1:5000/change_image", data=instruction ) self.logger.log_info( "Instruction " + str(instruction) + " sent to ui server" ) elif target == "ui_on_message": requests.post( "http://127.0.0.1:5000/on_message", data=instruction ) self.logger.log_info( "Instruction " + str(instruction) + " sent to ui server" ) elif target == "record_usage": requests.post( "http://127.0.0.1:5288/api/license/use") self.logger.log_info( "Instruction " + str(instruction) + " sent to license server" ) elif target == "check_license": try: response = requests.get("http://127.0.0.1:5288/api/license/check") response.raise_for_status() # Check for HTTP request errors (e.g., 404, 500) self.logger.log_info( "Instruction " + str(instruction) + " sent to license server" ) # Check if the response contains 'can_use' if 'can_use' in response.json(): self.license_check = copy.deepcopy(response.json().get("can_use")) else: raise ValueError("Response does not contain 'can_use' field") self.logger.log_info( "license check status is : " + str(self.license_check) ) except requests.exceptions.RequestException as e: self.license_check = False self.logger.log_error(f"Request failed: {e}") except ValueError as e: self.license_check = False self.logger.log_error(f"Value error: {e}") except Exception as e: self.license_check = False self.logger.log_error(f"An unexpected error occurred: {e}") elif target == "iot_massage_head": try: instruction['device_name'] = os.uname()[1] instruction['produce_name'] = "RS-LL-X1" response = requests.post( "http://app.robotstorm.tech:8080/iot/massageHead", json=instruction, timeout=5 # 设置5秒超时 ) response.raise_for_status() # 检查HTTP错误 self.logger.log_info( f"send iot_massage_head: {instruction}, status code: {response.status_code}" ) except Exception as e: self.logger.log_error(f"Failed to send massage head data to IoT platform: {e}") except Exception as e: self.logger.log_error(f"请求失败: {e}") def calculate_path_length(self,points: np.ndarray) -> float: """ 计算路径上所有点之间的总长度 :param points: 轨迹点的坐标数组 :return: 轨迹路径的总长度 """ length = 0.0 self.logger.log_blue(f"二维路径数组长度len(points):{len(points)}") for i in range(1, len(points)): # 计算两个连续点之间的欧几里得距离 length += np.abs(np.linalg.norm(np.array(points[i]) - np.array(points[i - 1]))) self.logger.log_blue(f"二维路径数组欧几里得距离length:{length}") self.path_length = length def generate_next_points(self,start_point,end_point,width,circles,direction:Literal["CW","CCW"] ="CW",path:Literal["line","in_spiral","out_spiral","ellipse","lemniscate","cycloid","point","point_rub"] ="line"): self.show_image_num += 1 self.logger.log_blue(f"start and end position:{start_point},{end_point}") if self.is_insert: self.logger.log_info("检测到插入队列，执行退出！") return if path == "line": # 循经直推法 temp_points = self.robot.generate_line_cloudPoint(start = start_point, end = end_point) elif path == "in_spiral": # 螺旋外散法 temp_points = self.robot.generate_in_spiral_cloudPoint(start = start_point, end = end_point, width = width, num_cycle_factor = circles, direction = direction) elif path == "out_spiral": # 螺旋内揉法 temp_points = self.robot.generate_out_spiral_cloudPoint(start = start_point, end = end_point, width = width, num_cycle_factor = circles, direction = direction) elif path == "ellipse": # 周天环摩法 temp_points = self.robot.generate_ellipse_cloudPoint(start = start_point, end = end_point, width = width, num_cycle_factor = circles, direction = direction) elif path == "lemniscate": # 双环疏经法 temp_points = self.robot.generate_Lemniscate_cloudPoint(start = start_point, end = end_point, width = width, num_cycle_factor = circles, direction = direction) elif path == "cycloid": # 摆浪通络法 temp_points = self.robot.generate_prolateCycloid_cloudPoint(start = start_point, end = end_point, width = width, num_cycle_factor = circles, direction = direction) elif path == "point" or path == "point_rub": # 定穴点按法 temp_points = [] temp_points.append(np.array(start_point)) temp_points = np.array(temp_points) else: self.logger.log_error("未找到該mode") self.send_instruction(instruction={"message": "检测到输入手法异常
并尝试重新设置"}, target="ui_on_message") raise StopWorkflow() if self.is_insert: self.logger.log_info("检测到插入队列，执行退出！") return if temp_points.size == 0: self.send_instruction(instruction={"message": "请检查是否存在不合理的参数设置或过短的按摩长度
并尝试重新设置"}, target="ui_on_message") self.logger.log_error("生成路径失败") raise StopWorkflow() self.next_points = self.coordinate_tf.pixel_to_robotarm(temp_points,self.show_image_num) if self.is_insert: self.logger.log_info("检测到插入队列，执行退出！") return if self.next_points is None: if self.body_part == "belly": self.send_instruction(instruction={"message": "您躺的位置有点不合适呢
请尝试往上躺一点呢"}, target="ui_on_message") else: self.send_instruction(instruction={"message": "您躺的位置有点不合适呢
请尝试往下躺一点呢"}, target="ui_on_message") raise StopWorkflow() else: if path != "point" and path != "point_rub": self.calculate_path_length(points = copy.deepcopy(temp_points)) else: self.path_length = 0 if self.is_insert: self.logger.log_info("检测到插入队列，执行退出！") return self.logger.log_info(f"当前转化路径模式{path}") self.logger.log_blue(f"self.path_length:{self.path_length}") self.logger.log_blue(f"生成下一个点位:{self.next_points[-1]}") def parse_point(self, point_str): """解析点字符串，支持 xxx、xxx&xxx、xxx&xxx@1/4，返回 np.array 或 None""" if point_str is None: return np.array(None) # 如果本身是 None，直接返回 None，不处理也不报错 point_str = point_str.strip() # 处理 xxx&xxx@1/4 形式 if '&' in point_str: ratio_value = 0.5 # 默认中点 # 检查是否带比例 if '@' in point_str: try: points_part, ratio_part = point_str.split('@') points = points_part.split('&') numerator, denominator = map(float, ratio_part.strip().split('/')) ratio_value = numerator / denominator except Exception as e: self.logger.log_error(f"Invalid ratio format in '{point_str}'") return 0 # 比例错误，返回 0（根据你之前返回 0 的风格） else: points = point_str.split('&') if len(points) != 2: self.logger.log_error(f"Invalid point format, expected two points in '{point_str}'") return None # 非法格式 p1_name, p2_name = points[0].strip(), points[1].strip() point1 = self.acupuncture_dict.get(p1_name) point2 = self.acupuncture_dict.get(p2_name) # 只检查未匹配的情况（按你原来逻辑） if p1_name and point1 is None: self.logger.log_error(f"No acupuncture point found for '{p1_name}'") return None if p2_name and point2 is None: self.logger.log_error(f"No acupuncture point found for '{p2_name}'") return None # 计算比例点 return np.array(point1) * (1 - ratio_value) + np.array(point2) * ratio_value else: # 单点情况 point = self.acupuncture_dict.get(point_str) if point is None: self.logger.log_error(f"No acupuncture point found for '{point_str}'") return 0 # 未匹配才返回 0 return np.array(point) # 定义一个处理点的函数 def handle_special_case(self, np_point, np_other_point, point_type): # 计算斜率和截距 if np.abs(np_point[0] - np_other_point[0]) < 0.001: # 判断是否为垂直线 # 垂直线的x值是固定的 self.logger.log_blue(f"{point_type}点是垂直线,x值固定为 {np_point[0]}") np_point_new = copy.deepcopy(np_point) # 使用与其他点的x值相同 np_point_new[0] = np_point[0] # 根据上下限调整y值 if np_point[1] <= self.coordinate_tf.camera_Upper_limit: np_point_new[1] = self.coordinate_tf.camera_Upper_limit + 3 elif np_point[1] >= self.coordinate_tf.camera_Lower_limit: np_point_new[1] = self.coordinate_tf.camera_Lower_limit - 3 else: # 计算斜率 m = (np_other_point[1] - np_point[1]) / (np_other_point[0] - np_point[0]) # 计算截距 b = np_point[1] - m * np_point[0] np_point_new = copy.deepcopy(np_point) # 根据上下限调整y值 if np_point[1] <= self.coordinate_tf.camera_Upper_limit: np_point_new[1] = self.coordinate_tf.camera_Upper_limit + 3 elif np_point[1] >= self.coordinate_tf.camera_Lower_limit: np_point_new[1] = self.coordinate_tf.camera_Lower_limit - 3 # 根据新的y值计算对应的x np_point_new[0] = (np_point_new[1] - b) / m return np_point_new def process_massage_plan(self, massage_type, body_part, plan_name): massage_plan = self.vtxdb.get("massage_plan", plan_name) if not massage_plan: self.logger.log_error(f"No massage plan found for {massage_type} - {body_part}") self.acupuncture_dict = None return 0 if massage_plan.get('choose_task') != massage_type or massage_plan.get('body_part') != body_part: self.logger.log_error(f"Wrong massage plan for {massage_type} - {body_part}") self.acupuncture_dict = None return 0 if self.body_part == "belly": self.robot.width_default = np.abs(self.acupuncture_dict.get('水分')[1] - self.acupuncture_dict.get('关元')[1]) elif self.body_part == "back" or self.body_part == "shoulder" or self.body_part == "back_shoulder" or self.body_part == "waist": self.robot.width_default = np.abs(self.acupuncture_dict.get('膈俞左')[0] - self.acupuncture_dict.get('膈关左')[0]) elif self.body_part == "leg": self.robot.width_default = np.abs(self.acupuncture_dict.get('委中左')[1] - self.acupuncture_dict.get('合阳左')[1]) task_plan = massage_plan.get('task_plan') last_end_point_raw = None for task in task_plan: start_point_raw = task.get('start_point') end_point_raw = task.get('end_point') if start_point_raw is None: start_point_raw = copy.deepcopy(last_end_point_raw) last_end_point_raw = copy.deepcopy(end_point_raw) start_point = self.parse_point(start_point_raw) if start_point is None: # 发现问题立即退出（按原逻辑） return 0 end_point = self.parse_point(end_point_raw) if end_point is None: return 0 if start_point.tolist() is not None: np_start_point = np.array(copy.deepcopy(start_point)) if end_point.tolist() is not None: np_end_point = np.array(copy.deepcopy(end_point)) # 检查起点和终点的深度范围 if np.logical_or(np_start_point[1] <= (self.coordinate_tf.camera_Upper_limit+2), np_start_point[1] >= (self.coordinate_tf.camera_Lower_limit-2)) and np.logical_or(np_end_point[1] <= (self.coordinate_tf.camera_Upper_limit+2), np_end_point[1] >= (self.coordinate_tf.camera_Lower_limit-2)): self.logger.log_error(f"传入起点:{start_point_raw}-{start_point}和终点:{end_point_raw}-{end_point}没有深度，消去该位置") continue elif np.logical_or(np_start_point[1] <= (self.coordinate_tf.camera_Upper_limit+2), np_start_point[1] >= (self.coordinate_tf.camera_Lower_limit-2)):# 如果起点没有深度，进行特殊处理 self.logger.log_blue(f"传入起点:{start_point_raw}-{start_point}没有深度，特殊处理该位置") start_point = self.handle_special_case(np_start_point, np_end_point, 'start') self.logger.log_blue(f"起点:{np_start_point}特殊处理成{start_point}，特殊处理该位置") elif np.logical_or(np_end_point[1] <= (self.coordinate_tf.camera_Upper_limit+2), np_end_point[1] >= (self.coordinate_tf.camera_Lower_limit-2)): # 如果终点没有深度，进行特殊处理 self.logger.log_blue(f"传入终点:{end_point_raw}-{end_point}没有深度，特殊处理该位置") end_point = self.handle_special_case(np_end_point, np_start_point, 'end') self.logger.log_blue(f"终点:{np_end_point}特殊处理成{end_point}，特殊处理该位置") path = task.get('path') massage_time = task.get('time') width = task.get('width') cycles = task.get('cycles') direction = task.get('direction') jump_mode = task.get('jump_mode') try: is_headOn = task.get('headOn') except: is_headOn = True if body_part == "shoulder": pathBodypart = body_part else: pathBodypart = None self.mode_commands_list.append([ copy.deepcopy(start_point), copy.deepcopy(end_point), width, cycles, direction, path, massage_time, None, pathBodypart, start_point_raw, end_point_raw, jump_mode, is_headOn ]) if self.mode_commands_list is None: self.send_instruction(instruction={"message": "没有检测到穴位点
请重新拍照"}, target="ui_on_message") return 0 return 1 def queue_final_deal(self, return_state): self.is_first_move = True ##第一次标志位给True try: jump_mode_dict = self.vtxdb.get("system_config", "jump_mode") self.jump_mode_default = jump_mode_dict.get('enabled', False) self.logger.log_blue(f"当前系统默认跳跃模式：{self.jump_mode_default}") except: self.jump_mode_default = True if self.choose_task == "finger" or self.choose_task == "shockwave": if self.body_part != "back_shoulder": self.mode_commands_list = self.mode_commands_list * 2 self.mode_commands_list_default = copy.deepcopy(self.mode_commands_list) self.mode_commands_list = self.mode_commands_list * max(1, int(self.loops)) # 执行多遍按摩时 copy n次任务队列 self.massage_task_num = len(self.mode_commands_list) + 1 # 记录任务长度 self.logger.log_blue(f"当前记录的任务队列为第: {self.current_queue_num}个") self.logger.log_blue(f"当前记录的move_count为第: {self.move_count}个") self.logger.log_blue(f"当前记录的motion_count为第: {self.motion_count}个") self.logger.log_blue(f"当前记录的图片序号为第: {self.show_image_num}个") self.logger.log_blue(f"当前记录的任务队列为: {self.massage_task_num}") self.logger.log_blue(f"下发运动计划总时间: {self.task_time}") self.logger.log_blue(f"当前运动时间: {self.current_time}") self.logger.log_blue(f"目标循环次数: {self.loops}") if self.is_pause: if self.current_queue_num > 0: self.logger.log_info(f"消去已执行完的任务:{self.current_queue_num}") startPos = self.mode_commands_list[self.current_queue_num][0] if startPos.tolist() is None: self.mode_commands_list[self.current_queue_num][0] = copy.deepcopy(self.mode_commands_list[self.current_queue_num-1][1]) for i in range(self.current_queue_num): self.mode_commands_list.popleft() self.move_count = copy.deepcopy(self.current_queue_num) self.motion_count = copy.deepcopy(self.current_queue_num) self.show_image_num = copy.deepcopy(self.current_queue_num) else: self.current_queue_num = -2 self.move_count = self.current_queue_num+2 self.motion_count = self.current_queue_num+2 self.show_image_num = self.current_queue_num+2 else: self.progress = 0 self.logger.log_blue(f"当前执行的任务队列为第: {self.current_queue_num}个") self.logger.log_blue(f"当前规划的任务队列为: {self.mode_commands_list}") self.acupuncture_dict = None return return_state def get_massage_plan_config(self): """获取按摩类型和身体部位的配置映射""" return { "ball": { "supported_parts": ["belly", "back", "waist"], "plan_prefix": "全能滚珠" }, "thermotherapy": { "supported_parts": ["belly", "back", "waist", "leg"], "plan_prefix": "深部热疗" }, "stone": { "supported_parts": ["belly", "back", "waist", "leg"], "plan_prefix": "温砭舒揉" }, "roller": { "supported_parts": ["back", "waist", "leg"], "plan_prefix": "滚滚刺疗" }, "spheres": { "supported_parts": ["back", "waist", "leg"], "plan_prefix": "天球滚捏" }, "heat": { "supported_parts": ["belly", "back", "waist", "leg"], "plan_prefix": "能量热疗" }, "shockwave": { "supported_parts": ["back", "waist", "leg", "shoulder", "back_shoulder"], "plan_prefix": "点阵按摩" }, "ion": { "supported_parts": ["back","belly"], "plan_prefix": "离子光灸" }, "finger": { "supported_parts": ["back", "waist", "leg", "shoulder", "back_shoulder"], "plan_prefix": "指疗通络" } } def get_plan_name(self, task_prefix, part_name, selected_plan=None): """ 生成或验证按摩方案名称 Args: task_prefix: 按摩类型前缀(如"深部热疗") part_name: 身体部位名称(如"背部") selected_plan: 用户选择的方案名称 Returns: str: 最终使用的方案名称 """ default_plan = f"{task_prefix}-{part_name}-默认" if not selected_plan: return default_plan # 解析选择的方案 parts = selected_plan.split("-") if len(parts) != 3: self.logger.log_error(f"选择的方案格式不正确: {selected_plan}") return default_plan # 验证前缀和部位是否匹配 if parts[0] != task_prefix or parts[1] != part_name: self.logger.log_error(f"选择的方案与当前按摩类型或部位不匹配: {selected_plan}") return default_plan return selected_plan def process_massage_task(self): """处理按摩任务""" massage_config = self.get_massage_plan_config() # 检查按摩类型是否支持 if self.choose_task not in massage_config: self.manual_start = False self.logger.log_error(f"未知的按摩类型 {self.choose_task}") return "stop" task_config = massage_config[self.choose_task] body_part = self.body_part # 检查身体部位是否支持 if body_part not in task_config["supported_parts"]: self.manual_start = False self.logger.log_error(f"{self.choose_task} 不支持按摩部位 -{body_part}-") return None # 特殊处理背部和肩部组合按摩 if body_part == "back_shoulder": result1 = self.process_massage_plan( self.choose_task, "shoulder", self.get_plan_name(task_config["plan_prefix"], "肩颈", self.selected_plan) ) result2 = self.process_massage_plan( self.choose_task, "back", self.get_plan_name(task_config["plan_prefix"], "背部", self.selected_plan) ) if result1 and result2: return self.queue_final_deal(f"to_{self.choose_task}") else: # 普通部位按摩处理 # part_name = "腹部" if body_part == "belly" else "背部" if body_part == "back" else "肩颈" part_name = ( "腹部" if body_part == "belly" else "背部" if body_part == "back" else "肩颈" if body_part == "shoulder" else "腰部" if body_part == "waist" else "腿部" ) plan_name = self.get_plan_name( task_config["plan_prefix"], part_name, self.selected_plan ) if self.process_massage_plan(self.choose_task, body_part, plan_name): # 获取当前时间并格式化为指定格式 current_time = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") # 构建按摩头数据 instruction = { "start_time": current_time, "task_time": self.task_time, "head_type": self.choose_task, "body_part": body_part } self.send_instruction(instruction, target="iot_massage_head") return self.queue_final_deal(f"to_{self.choose_task}") self.manual_start = False return None def update_massageHead_temper(self): task_map = { "stone": self.robot.stone # "stone": self.robot.stone, # "heat": self.robot.heat } while self.choose_task is not None: try: device = task_map.get(self.choose_task) if device and device.isOn: response = device.get_temp() self.logger.log_info(f"读取到的按摩头温度：{response}") self.temper_head = response else: self.temper_head = None except Exception: self.temper_head = None if self.temper_head is not None: instruction = { "temperature": self.temperature, "temper_head": self.temper_head } self.logger.log_yellow(instruction) self.send_instruction(instruction, "ui_update_massage_status") time.sleep(5) def init_massage_parameters(self): if self.choose_task == "thermotherapy": config = self.vtxdb.get("robot_config","massage_head.thermotherapy_head") temperature_limit = config.get('temperature_limit', None) gear_limit = config.get('gear_limit', None) shake_limit = config.get('shake_limit', None) if temperature_limit is None or gear_limit is None or shake_limit is None: self.logger.log_info("limit is none,please check the input") return 'pause' if self.is_pause else 'stop' self.temperature_limit = temperature_limit self.gear_limit = gear_limit self.shake_limit = shake_limit self.robot.thermotherapy = Thermotherapy() if self.is_pause == False: self.temperature = self.temperature_default_thermotherapy self.gear = self.gear_default self.shake = self.shake_default if self.body_part == "belly": self.robot.arm_state.massage_wrench = np.array([0, 0, -10, 0, 0, 0]) else: self.robot.arm_state.massage_wrench = np.array([0, 0, -20, 0, 0, 0]) self.logger.log_info('启动热疗按摩') for _ in range(3): self.robot.thermotherapy.start_process(); time.sleep(0.5) self.robot.thermotherapy.set_working_status(heat_level=self.temperature) elif self.choose_task == "stone": config = self.vtxdb.get("robot_config","massage_head.stone_head") temperature_limit = config.get('temperature_limit', None) speed_limit = config.get('speed_limit', None) if temperature_limit is None or speed_limit is None: self.logger.log_info("limit is none,please check the input") return "stop" self.temperature_limit = temperature_limit self.speed_limit = speed_limit self.robot.stone = Stone() if self.is_pause == False: self.temperature = self.temperature_default_stone self.speed = self.speed_default self.direction = self.direction_default if self.body_part == "belly": self.robot.arm_state.massage_wrench = np.array([0, 0, -10, 0, 0, 0]) else: self.robot.arm_state.massage_wrench = np.array([0, 0, -20, 0, 0, 0]) self.logger.log_info('启动砭石按摩') for _ in range(3): self.robot.stone.power_on(); time.sleep(0.5) self.robot.stone.set_working_status(heat_level=self.temperature) if self.update_massageHead_temper_thread is None: self.update_massageHead_temper_thread = threading.Thread(target=self.update_massageHead_temper) self.update_massageHead_temper_thread.start() elif self.choose_task == "heat": config = self.vtxdb.get("robot_config","massage_head.heat_head") temperature_limit = config.get('temperature_limit', None) if temperature_limit is None: self.logger.log_info("limit is none,please check the input") return "stop" self.temperature_limit = temperature_limit self.robot.heat = Heat() if self.is_pause == False: self.temperature = self.temperature_default_heat if self.body_part == "belly": self.massage_wrench = np.array([0, 0, -10, 0, 0, 0]) else: self.massage_wrench = np.array([0, 0, -20, 0, 0, 0]) self.logger.log_info('启动能量按摩') for _ in range(3): self.robot.heat.power_on(); time.sleep(0.5) self.robot.heat.set_working_status(heat_level=self.temperature) if self.update_massageHead_temper_thread is None: self.update_massageHead_temper_thread = threading.Thread(target=self.update_massageHead_temper) self.update_massageHead_temper_thread.start() elif self.choose_task == "shockwave": config = self.vtxdb.get("robot_config","massage_head.shockwave_head") press_limit = config.get('press_limit', None) frequency_limit = config.get('frequency_limit', None) if press_limit is None or frequency_limit is None: self.logger.log_info("limit is none,please check the input") return "stop" self.press_limit = press_limit self.frequency_limit = frequency_limit self.logger.log_info('启动点阵按摩') self.robot.shockwave = Shockwave() if self.is_pause == False: self.press = self.press_default self.frequency = self.frequency_default self.robot.arm_state.massage_wrench = np.array([0, 0, -25, 0, 0, 0]) elif self.choose_task == "ball": if self.is_pause == False: self.robot.arm_state.massage_wrench = np.array([0, 0, -15, 0, 0, 0]) elif self.choose_task == "finger": if self.is_pause == False: self.robot.arm_state.massage_wrench = np.array([0, 0, -25, 0, 0, 0]) elif self.choose_task == "roller": if self.is_pause == False: self.robot.arm_state.massage_wrench = np.array([0, 0, -25, 0, 0, 0]) elif self.choose_task == "spheres": if self.is_pause == False: self.robot.arm_state.massage_wrench = np.array([0, 0, -25, 0, 0, 0]) elif self.choose_task == "ion": config = self.vtxdb.get("robot_config","massage_head.ion_head") temperature_limit = config.get('temperature_limit', None) high_limit = config.get('high_limit', None) if temperature_limit is None or high_limit is None: self.logger.log_info("limit is none,please check the input") return "stop" self.robot.ion = Ion() self.temperature_limit = temperature_limit self.high_limit = high_limit self.logger.log_info('启动离子按摩') if self.is_pause == False: self.temperature = self.temperature_default_ion self.robot.arm_state.desired_high = self.desired_high_default self.robot.arm_state.massage_wrench = np.array([0, 0, -5, 0, 0, 0]) return None def init_queue_para(self): self.move_count = 0 self.motion_count = 0 self.show_image_num = 0 self.plan_task = None self.motion_task = None self.current_queue_num = -2 self.insert_len = 0 self.mode_commands_list = deque([]) self.mode_commands_list_default = deque([]) self.target_points = None self.is_skip = False def move_camera_position(self): #运动到拍照位置 ## 测试时使用的注释程序 self.tool_cam = ToolCamera(host="127.0.0.1") self.tool_cam.start() time.sleep(1) self.logger.log_yellow("运动到拍照位置") self.robot.arm.disable_servo() time.sleep(0.5) # code = self.robot.arm.move_joint(self.robot.arm.cam_pose, max_retry_count = 4,wait=True) code = self.robot.set_joint(joint = self.robot.arm.cam_pose) if code == -1: self.logger.log_error("运动到拍照位置失败") self.stop_event.set() return "emergency" time.sleep(1) self.cam_position,self.cam_quaternion = self.robot.arm.get_end_position() self.logger.log_info(f"获取cam的位置:{self.cam_position}和姿态:{self.cam_quaternion}") if self.stop_event.is_set(): self.logger.log_info(f"检测到停止事件") return "stop" if self.pause_event.is_set(): self.pause_event.clear() self.send_instruction(instruction="no_pause", target="language") self.send_instruction(instruction={"message": "我现在在拍照呢
暂时不可以暂停哦"}, target="ui_on_message") return None def process_take_photo(self): #拍照 self.send_instruction(instruction="take_a_photo", target="language") # 最大重试次数 MAX_RETRIES = 5 # 初始化重试计数器 retry_count = 0 while retry_count < MAX_RETRIES: # 尝试获取数据 color_image, depth_image, intrinsics = self.tool_cam.get_latest_frame() # 检查获取到的内容是否为 None if color_image is not None and depth_image is not None and intrinsics is not None: # 如果所有数据都成功获取，则退出循环 break # 如果获取失败，增加重试计数器 retry_count += 1 self.logger.log_warning(f"获取数据失败，正在重试 {retry_count}/{MAX_RETRIES}...") self.tool_cam.start() time.sleep(1) # 检查是否超出重试次数 if retry_count == MAX_RETRIES: self.logger.log_warning("获取数据失败，已达到最大重试次数。") else: self.logger.log_info("拍照成功。") cali_intrinsics = self.vtxdb.get("robot_config","camera.intrinsics") if cali_intrinsics is not None: self.intrinsics = cali_intrinsics else: self.intrinsics = copy.deepcopy(intrinsics) self.logger.log_info("self.intrinsics:" + str(self.intrinsics)) # 获取当前时间并格式化为文件夹名 (格式: 年-月-日_时-分-秒) current_time = time.strftime("%Y-%m-%d_%H-%M-%S") current_time = f"{current_time}_{self.body_part}" # 判断 mode_real 并保存图像 if self.mode_real == 1 or self.mode_real == 3: # 根据 mode_real 来设置图片保存的基础路径 if self.mode_real == 1: base_dir = "/home/jsfb/jsfb_ws/collected_data/images" else: base_dir = "/home/jsfb/jsfb_ws/test_data/test_images" # 创建一个根据时间命名的新文件夹 folder_path = os.path.join(base_dir, current_time) os.makedirs(folder_path, exist_ok=True) # 如果文件夹不存在则创建 try: # 保存 color 和 depth 图像到文件 cv2.imwrite(os.path.join(folder_path, "color.png"), color_image) cv2.imwrite(os.path.join(folder_path, "depth.png"), depth_image) # 另外保存到固定位置 cv2.imwrite("aucpuncture2point/configs/using_img/color.png", color_image) cv2.imwrite("aucpuncture2point/configs/using_img/depth.png", depth_image) # 保存相机内参到文件 with open(os.path.join(folder_path, "intrinsics.json"), "w") as f: json.dump(self.intrinsics, f) if self.mode_real == 3: # 定义基础路径 if self.body_part == "belly": base_path = "/home/jsfb/jsfb_ws/test_data/belly_images/" elif self.body_part == "leg": base_path = '/home/jsfb/jsfb_ws/test_data/leg_images/' else: base_path = '/home/jsfb/jsfb_ws/test_data/images/' # 获取所有子文件夹的列表，并按修改时间排序 subfolders = sorted( [f.path for f in os.scandir(base_path) if f.is_dir()], key=lambda x: os.path.getmtime(x) # 根据修改时间排序 ) # 打印出所有子文件夹的名称（按时间顺序） print("Subfolders sorted by modification time:") for folder in subfolders: print(folder) # 按照时间顺序选择文件夹 # 检查是否达到最后一个文件夹，如果是则从头开始 selected_subfolder = subfolders[self.broken_test] if self.broken_test == len(subfolders) - 1: self.broken_test = 0 # 重置为第一个文件夹 else: self.broken_test += 1 # 否则，选择下一个文件夹 # 打印出选择的文件夹 print(f"\nSelected subfolder (earliest modified): {selected_subfolder}") # 拼接color.png和depth.png的路径 img_source = os.path.join(selected_subfolder, 'color.png') depth_source = os.path.join(selected_subfolder, 'depth.png') # 目标路径 img_destination = 'aucpuncture2point/configs/using_img/color.png' depth_destination = 'aucpuncture2point/configs/using_img/depth.png' try: # 使用 shutil 复制文件 shutil.copy(img_source, img_destination) shutil.copy(depth_source, depth_destination) except Exception as e: print(f"Error occurred while copying test images: {e}") return 'stop' except Exception as e: # 打印异常信息并返回 'stop' print(f"Error occurred while saving images: {e}") return 'pause' if self.is_pause else 'stop' else: base_dir = "/home/jsfb/jsfb_ws/collected_data/images" # 创建一个根据时间命名的新文件夹 folder_path = os.path.join(base_dir, current_time) os.makedirs(folder_path, exist_ok=True) # 如果文件夹不存在则创建 # 根据 body_part 来选择图像源 if self.body_part == "belly": img_source = '/home/jsfb/jsfb_ws/test_data/test_front/color.png' depth_source = '/home/jsfb/jsfb_ws/test_data/test_front/depth.png' elif self.body_part == "leg": img_source = "/home/jsfb/jsfb_ws/test_data/test_leg/color.png" depth_source = "/home/jsfb/jsfb_ws/test_data/test_leg/depth.png" else: img_source = '/home/jsfb/jsfb_ws/test_data/test_back/color.png' depth_source = '/home/jsfb/jsfb_ws/test_data/test_back/depth.png' # 目标路径 img_destination = 'aucpuncture2point/configs/using_img/color.png' depth_destination = 'aucpuncture2point/configs/using_img/depth.png' try: # 使用 shutil 复制文件 shutil.copy(img_source, img_destination) shutil.copy(depth_source, depth_destination) except Exception as e: print(f"Error occurred while copying test images: {e}") return 'stop' if self.use_mode == "manual": instruction = {"path": os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/color.png","type":self.show_picture_type} self.send_instruction(instruction=instruction, target="ui_change_image") color_image = cv2.imread('aucpuncture2point/configs/using_img/color.png') depth_image = cv2.imread('aucpuncture2point/configs/using_img/depth.png', cv2.IMREAD_UNCHANGED) self.coordinate_tf = CoordinateTransformer(self.intrinsics,color_image,depth_image,self.cam_position,self.cam_quaternion,self.vtxdb) if self.use_mode == "manual": self.manual_stage = 1 self.send_instruction({"manual_stage": self.manual_stage}, "ui_update_massage_status") self.get_picture = False return None def process_acupoint_recognize(self): # 处理按摩点识别 self.send_instruction(instruction="acupoint_recognize", target="language") if self.acupoint_key_data is not None: self.logger.log_info("手动计算穴位") if self.body_part == "belly": self.logger.log_info("计算腹部穴位") self.acupuncture_dict = self.abdomen_detector.detect_by_manual(image_path='aucpuncture2point/configs/using_img/color.png',output_path='aucpuncture2point/configs/using_img/abdomen_img/abdomen_acupoints.png',json_data=self.acupoint_key_data) elif self.body_part == "back" or self.body_part == "shoulder" or self.body_part == "back_shoulder" or self.body_part == "waist": self.logger.log_info("计算背部穴位") self.acupuncture_dict = self.back_detector.detect_by_manual(image_path='aucpuncture2point/configs/using_img/color.png',output_path='aucpuncture2point/configs/using_img/back_img/back_acupoints.png',json_data=self.acupoint_key_data) elif self.body_part == "leg": self.logger.log_info("计算腿部穴位") self.acupuncture_dict = self.leg_detector.detect_by_manual(image_path='aucpuncture2point/configs/using_img/color.png',output_path='aucpuncture2point/configs/using_img/leg_img/leg_acupoints.png',json_data=self.acupoint_key_data) else: self.logger.log_error("未知的身体部位") self.send_instruction(instruction={"message": "未知的身体部位
请重新选择"}, target="ui_on_message") return 'stop' else: self.logger.log_info("自动计算穴位") if self.body_part == "belly": self.logger.log_info("计算腹部穴位") self.acupuncture_dict = self.abdomen_detector.detect_by_yolo(image_path='aucpuncture2point/configs/using_img/color.png',output_path='aucpuncture2point/configs/using_img/abdomen_img/abdomen_acupoints.png') elif self.body_part == "back" or self.body_part == "shoulder" or self.body_part == "back_shoulder" or self.body_part == "waist": self.logger.log_info("计算背部穴位") self.acupuncture_dict = self.back_detector.detect_by_yolo(image_path='aucpuncture2point/configs/using_img/color.png',output_path='aucpuncture2point/configs/using_img/back_img/back_acupoints.png') elif self.body_part == "leg": self.logger.log_info("计算腿部穴位") self.acupuncture_dict = self.leg_detector.detect_by_yolo(image_path='aucpuncture2point/configs/using_img/color.png',output_path='aucpuncture2point/configs/using_img/leg_img/leg_acupoints.png') else: self.logger.log_error("未知的身体部位") self.send_instruction(instruction={"message": "未知的身体部位
请重新选择"}, target="ui_on_message") return 'stop' if self.acupuncture_dict is not None: # 将 acupoints 中的所有 tuple 值转换为 list for key, value in self.acupuncture_dict.items(): if isinstance(value, tuple): # 检查值是否为 tuple self.acupuncture_dict[key] = list(value) # 转换为 list self.logger.log_info(f"self.acupuncture_dict{self.acupuncture_dict}") self.cal_acu = False self.acupoint_key_data = None if self.body_part == "belly": instruction = {"path": os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/abdomen_img/abdomen_acupoints.png","type":3} elif self.body_part == "back" or self.body_part == "shoulder" or self.body_part == "back_shoulder" or self.body_part == "waist": instruction = {"path": os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/back_img/back_acupoints.png","type":3} elif self.body_part == "leg": instruction = {"path": os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/leg_img/leg_acupoints.png","type":3} self.send_instruction(instruction=instruction, target="ui_change_image") if self.coordinate_tf: if self.body_part == "belly": self.coordinate_tf.color_visualize = cv2.imread(os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/abdomen_img/abdomen_acupoints.png") elif self.body_part == "back" or self.body_part == "shoulder" or self.body_part == "back_shoulder" or self.body_part == "waist": self.coordinate_tf.color_visualize = cv2.imread(os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/back_img/back_acupoints.png") elif self.body_part == "leg": self.coordinate_tf.color_visualize = cv2.imread(os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/leg_img/leg_acupoints.png") else: self.logger.log_error("坐标转换器未初始化") instruction = {} self.is_acupoint = True instruction["is_acupoint"] = self.is_acupoint self.logger.log_yellow(instruction) self.send_instruction(instruction, "ui_update_massage_status") else: if self.use_mode == "smart": self.logger.log_error("沒有檢測到穴位") self.send_instruction(instruction={"message": "没有检测到穴位点
请重新拍照"}, target="ui_on_message") return 'pause' if self.is_pause else 'stop' elif self.use_mode == "manual": self.logger.log_error("没有检测到关键点") self.send_instruction(instruction={"message": "没有检测到穴位点
请重新拍照"}, target="ui_on_message") self.cal_acu = False return None def process_ready_massage(self): # 处理准备按摩 if self.use_mode == "manual": self.manual_stage = 2 self.send_instruction({"manual_stage": self.manual_stage}, "ui_update_massage_status") if self.tool_cam is not None: self.tool_cam.stop() pose = copy.deepcopy(self.robot.arm.standby_pos) if self.stop_event.is_set(): return "stop" if self.pause_event.is_set(): self.pause_event.clear() self.send_instruction(instruction="no_pause", target="language") self.send_instruction(instruction={"message": "即将开始按摩
暂时不可以暂停哦"}, target="ui_on_message") self.robot.is_waitting = True self.robot.arm.disable_servo() time.sleep(1) # code = self.robot.arm.move_joint(pose, max_retry_count = 4) code = self.robot.set_joint(joint = pose) if code == -1: self.logger.log_error("运动到standby位置失败") self.stop_event.set() return "emergency" time.sleep(1) if self.stop_event.is_set(): return "stop" if self.pause_event.is_set(): self.pause_event.clear() self.send_instruction(instruction="no_pause", target="language") self.send_instruction(instruction={"message": "即将开始按摩
暂时不可以暂停哦"}, target="ui_on_message") self.robot.is_waitting = False self.logger.log_info("执行预处理") self.robot.arm_state.desired_wrench = np.zeros(6,dtype=np.float64) # 重置期望力矩 self.is_finish = False #重置任务完成标志 #是否切换控制方式 self.is_switch_controller = self.mode_real != 0 if self.choose_task == "shockwave" or self.choose_task == "finger": self.use_algorithm = "position" if self.mode_real == 3 else ("hybridAdmit" if self.is_switch_controller else "admittance") elif self.choose_task == "ion": self.use_algorithm = "positionerSensor" else: self.use_algorithm = "position" if self.mode_real == 3 else ("hybrid" if self.is_switch_controller else "admittance") return None def tf2base(self, offset, rpy): # 创建从 rpy (roll, pitch, yaw) 生成的旋转对象 rotation = R.from_euler("xyz", rpy) # 将偏移应用到旋转矩阵，转换到基坐标系 base_offset = rotation.apply(offset) return base_offset def process_insert_queue(self): if self.generate_thread is not None: self.generate_thread.join() self.generate_thread = None self.coordinate_tf.insert_queue = False #重置视觉插入队列标志位 self.show_image_num = self.motion_count #重置图片编号 self.current_queue_num = self.motion_count-2 if self.plan_task is not None: self.mode_commands_list.appendleft(self.plan_task) turn_motion_task = copy.deepcopy(self.motion_task) turn_motion_task[0] = copy.deepcopy(self.motion_task[1]) turn_motion_task[1] = copy.deepcopy(self.motion_task[0]) if self.motion_task[5] == "in_spiral": #如果当前运动任务为螺旋线，则特殊处理当前螺旋线方式 turn_motion_task[5] = "out_spiral" elif self.motion_task[5] == "out_spiral": turn_motion_task[5] = "in_spiral" self.mode_commands_list.appendleft(self.motion_task) self.mode_commands_list.appendleft(turn_motion_task) self.insert_len = 2 self.massage_task_num += 2 # 获取任务信息 self.plan_task = self.mode_commands_list.popleft() self.insert_len -= 1 self.logger.log_info(f"plan_task:{self.plan_task}") if self.plan_task[0].tolist() is None: if self.plan_task[5] == "point" or self.plan_task[5] == "point_rub": self.plan_task[0] = copy.deepcopy(self.plan_task[1]) # 更新任务队列 else: self.plan_task[0] = copy.deepcopy(self.motion_task[1]) # 更新任务队列 self.logger.log_blue(f"修正 startPos:{self.plan_task[0]}") # 启动新线程 self.logger.log_info(f"开始规划 {self.plan_task} 路线") self.is_insert = False #重置插入标志位 self.generate_thread = threading.Thread(target=self.generate_next_points, args=(self.plan_task[0],self.plan_task[1],self.plan_task[2],self.plan_task[3]),kwargs={'direction':self.plan_task[4],'path':self.plan_task[5]}) self.generate_thread.start() #准备处理之前插入 if self.plan_task[0] is not None and not np.allclose(self.plan_task[0], self.motion_task[1], atol=0.1): # 跳跃准备 self.is_jump = True self.jump_length = np.linalg.norm(self.plan_task[0] - self.motion_task[1]) def update_time(self): self.start_time = time.time() - self.current_time while self.is_finish == False: self.current_time = time.time() - self.start_time #更新时间 if self.loops != 0: self.send_instruction({"progress": str(round((self.move_count/self.massage_task_num)*100))}, "ui_update_massage_status") else: if self.task_time > 0: self.send_instruction({"progress": str(round((self.current_time/self.task_time)*100))}, "ui_update_massage_status") else: self.send_instruction({"progress": str(round(0))}, "ui_update_massage_status") if self.current_time > self.task_time: self.is_finish = True time.sleep(5) if self.loops == 0: self.logger.log_info(f"按摩时间到{self.current_time},{self.task_time}") self.is_finish = True else: self.logger.log_info(f"当前控制为循环几次：{self.loops}") class SMART_DEAL(smach.State): def __init__(self, resources: SharedResources): smach.State.__init__( self, outcomes=[ "to_thermotherapy", "to_shockwave", "to_ball", "to_finger", "to_roller", "to_spheres", "to_stone", "to_heat", "to_ion", "pause", "stop", "emergency", ], ) self.resources = resources def execute(self, userdata): self.resources.robot.is_waitting = True try: #初始化参数 if (code := self.resources.init_massage_parameters()): return code except Exception as e: self.resources.send_instruction(instruction={"message": "按摩头开启失败
请检查按摩头及连接状态"}, target="ui_on_message") self.resources.logger.log_error(f'启动按摩头错误: {e}') return "stop" instruction = {} instruction["massage_service_started"] = True instruction["is_massaging"] = (True) instruction["massage_head"] = (self.resources.robot.current_head) instruction["progress"] = str(self.resources.progress) instruction["force"] = str(-self.resources.robot.arm_state.massage_wrench[2]) instruction["current_head"] = self.resources.robot.current_head instruction["temperature"] = self.resources.temperature instruction["gear"] = self.resources.gear instruction["shake"] = self.resources.shake instruction["press"] = self.resources.press instruction["high"] = int(self.resources.robot.arm_state.desired_high * 100) instruction["frequency"] = self.resources.frequency instruction["task_time"] = str(self.resources.task_time) instruction["current_time"] = str(self.resources.current_time) instruction["loops"] = str(self.resources.loops) instruction["body_part"] = self.resources.body_part instruction["mode_real"] = self.resources.mode_real instruction["massage_state"] = self.resources.now_state instruction["manual_stage"] = self.resources.manual_stage instruction["temper_head"] = None self.resources.logger.log_yellow(instruction) self.resources.send_instruction(instruction, "ui_update_massage_status") if (code := self.resources.move_camera_position()): #运动到拍照位置 return code if (code := self.resources.process_take_photo()): #运动到拍照位置 return code if (code := self.resources.process_acupoint_recognize()): #穴位识别 return code if (code := self.resources.process_ready_massage()): #准备按摩 return code # 在原代码中替换整个 if-elif 链 result = self.resources.process_massage_task() if result: return result return 'pause' if self.resources.is_pause else 'stop' class MANUAL_DEAL(smach.State): def __init__(self, resources: SharedResources): smach.State.__init__( self, outcomes=[ "to_thermotherapy", "to_shockwave", "to_ball", "to_finger", "to_roller", "to_spheres", "to_stone", "to_heat", "pause", "stop", "to_ion", "emergency", ], ) self.resources = resources def execute(self, userdata): self.resources.robot.is_waitting = True try: code = self.resources.init_massage_parameters() if code is not None: return code except Exception as e: self.resources.send_instruction(instruction={"message": "按摩头开启失败
请检查按摩头及连接状态"}, target="ui_on_message") self.resources.logger.log_error(f'启动按摩头错误: {e}') return "stop" instruction = {} instruction["massage_service_started"] = True instruction["is_massaging"] = (True) instruction["massage_head"] = (self.resources.robot.current_head) instruction["progress"] = str(self.resources.progress) instruction["force"] = str(-self.resources.robot.arm_state.massage_wrench[2]) instruction["current_head"] = self.resources.robot.current_head instruction["temperature"] = self.resources.temperature instruction["gear"] = self.resources.gear instruction["shake"] = self.resources.shake instruction["press"] = self.resources.press instruction["high"] = int(self.resources.robot.arm_state.desired_high * 100) instruction["frequency"] = self.resources.frequency instruction["task_time"] = str(self.resources.task_time) instruction["current_time"] = str(self.resources.current_time) instruction["loops"] = str(self.resources.loops) instruction["body_part"] = self.resources.body_part instruction["mode_real"] = self.resources.mode_real instruction["massage_state"] = self.resources.now_state instruction["manual_stage"] = self.resources.manual_stage self.resources.logger.log_yellow(instruction) self.resources.send_instruction(instruction, "ui_update_massage_status") if (code := self.resources.move_camera_position()): #运动到拍照位置 return code while not self.resources.stop_event.is_set(): if self.resources.get_picture == True: if (code := self.resources.process_take_photo()): #执行拍照 return code elif self.resources.cal_acu == True: if (code := self.resources.process_acupoint_recognize()): #穴位识别 return code elif self.resources.manual_start == True and self.resources.acupuncture_dict is not None: if (code := self.resources.process_ready_massage()): #准备按摩 return code # 在原代码中替换整个 if-elif 链 result = self.resources.process_massage_task() if result: return result return 'pause' if self.resources.is_pause else 'stop' if self.resources.stop_event.is_set(): if self.resources.tool_cam is not None: self.resources.tool_cam.stop() return "stop" if self.resources.tool_cam is not None: self.resources.tool_cam.stop() return 'stop' class TaskBase(smach.State): def __init__(self, resources: SharedResources): smach.State.__init__(self, outcomes=["pause","stop", "emergency"]) self.resources = resources self.admit_begin_pos = None self.start_time = 10 self.turn = [0, 0, 45*np.pi/180] self.turn_2D = [0, 0, 30*np.pi/180] self.turn_ion = [0, 0, -45*np.pi/180] self.back_angle = R.from_matrix(R.from_euler("xyz",self.resources.robot.arm.level_base,degrees=False).as_matrix() @ R.from_euler("xyz",self.turn,degrees=False).as_matrix()).as_euler("xyz",degrees=False) self.back_angle_ion = R.from_matrix(R.from_euler("xyz",self.resources.robot.arm.level_base,degrees=False).as_matrix() @ R.from_euler("xyz",self.turn_ion,degrees=False).as_matrix()).as_euler("xyz",degrees=False) def organize_dict(self, dictionary, substrings): raise NotImplementedError("You should implement this method in derived classes") def process_except(self, code): """处理异常,返回值为2或4时结束并返回原位,返回值为3时紧急停止,检测到调整事件时触发调整异常,return0表示正常运行,return1表示异常运行""" if code == 3: raise EmergencyStopWorkflow() if self.resources.stop_event.is_set() or code == 4 or code == 2: if code == 2: self.resources.send_instruction(instruction="record_usage", target="record_usage") raise StopWorkflow() if self.resources.is_finish == False: if self.resources.pause_event.is_set() or code == 5: raise PauseWorkflow() if self.resources.is_finish == False: if self.resources.skip_event.is_set() or code == 6: time.sleep(0.2) position,quat_rot = self.resources.robot.arm.get_arm_position() time.sleep(0.2) target_point = np.zeros(6) target_point[:3] = copy.deepcopy(position) target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False)) target_point[3:] = self.resources.robot.arm.level_base target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.10], self.resources.robot.arm.level_base) self.resources.robot.skip_pos = copy.deepcopy(target_point) self.resources.logger.log_blue(f"检测到跳过，正在处理跳过起始点: {self.resources.robot.skip_pos}") return 0 if self.resources.adjust_event.is_set(): self.adjust_value() return 1 return 0 def adjust_value(self): raise NotImplementedError("You should implement this method in derived classes") def execute(self, userdata): raise NotImplementedError("You should implement this method in derived classes") def tf2base(self, offset, rpy): # 创建从 rpy (roll, pitch, yaw) 生成的旋转对象 rotation = R.from_euler("xyz", rpy) # 将偏移应用到旋转矩阵，转换到基坐标系 base_offset = rotation.apply(offset) return base_offset def adjust_value(self): pass def write_to_json(self, filename): # Helper function to convert numpy arrays to lists def convert_numpy(obj): if isinstance(obj, np.ndarray): return obj.tolist() elif isinstance(obj, (list, tuple)): return [convert_numpy(x) for x in obj] elif isinstance(obj, dict): return {k: convert_numpy(v) for k, v in obj.items()} return obj # Get motion data (ensure NumPy arrays are converted) motion_data = { "startPos": convert_numpy(self.resources.motion_task[9]), "endPos": convert_numpy(self.resources.motion_task[10]), "massagePath": convert_numpy(self.resources.motion_task[5]), "massage_wrench": convert_numpy(self.resources.robot.arm_state.massage_wrench), "massage_head": self.resources.choose_task, "massage_plan": self.resources.selected_plan, "task_time": self.resources.task_time, "current_time": datetime.datetime.now().strftime("%Y-%m-%d_%H%M%S") } # Define the directory path directory = "/home/jsfb/jsfb_ws/collected_data/message" # Ensure the directory exists if not os.path.exists(directory): os.makedirs(directory) # Construct full file path filepath = os.path.join(directory, filename) # Load existing data (if file exists) or initialize a new list if os.path.exists(filepath): with open(filepath, 'r', encoding='utf-8') as f: existing_data = json.load(f) else: existing_data = [] # Append new data existing_data.append(motion_data) # Write back to file with open(filepath, 'w', encoding='utf-8') as f: json.dump(existing_data, f, ensure_ascii=False, indent=4) def process_jump_deal(self, jump_point): self.resources.logger.log_yellow(f"jump_point: {jump_point}{self.resources.jump_length}") # 跳跃处理 self.resources.logger.log_blue("开始处理跳跃") jump_time = max(1, min(60, self.resources.jump_length/(self.resources.jump_velocity))) if self.resources.choose_task == "shockwave": self.resources.robot.shockwave.off() # self.resources.send_instruction(instruction="change_position", target="language") if self.resources.mode_real != 0 and self.resources.mode_real != 3: massage_wrench = np.array([0, 0, 0, 0, 0, 0]) code = self.resources.robot.apply_force(wrench=massage_wrench, t=1, algorithm=self.resources.use_algorithm) self.process_except(code) if self.resources.jump_mode: if self.resources.choose_task == "ion": up_target_point = copy.deepcopy(jump_point) up_target_point[3:] = copy.deepcopy(self.back_angle_ion) up_target_point[:3] = up_target_point[:3] + self.tf2base([0, 0, -self.resources.robot.arm_state.desired_high], self.back_angle_ion) else: up_target_point = copy.deepcopy(jump_point) up_target_point[3:] = self.resources.robot.arm.level_base up_target_point[:3] = up_target_point[:3] + self.tf2base([0, 0, -0.10], self.resources.robot.arm.level_base) print("up_target_point:",up_target_point) if self.resources.choose_task == "shockwave" or self.resources.choose_task == "ion": code = self.resources.robot.move_to_point(pose=up_target_point, t=jump_time, timeout=0.5, algorithm="admithybrid", is_interrupt=False,is_switch_controller=True) else: code = self.resources.robot.move_to_point(pose=up_target_point, t=jump_time, timeout=0.5, algorithm="admittance", is_interrupt=False,is_switch_controller=True) self.process_except(code) jump_point[:3] = jump_point[:3] + self.tf2base([0, 0, 0.015], self.resources.robot.arm.level_base) if self.resources.choose_task == "shockwave" or self.resources.choose_task == "ion": code = self.resources.robot.move_to_point(pose=jump_point, t=2.5, timeout=0.5, algorithm='admithybrid',is_interrupt=False) else: code = self.resources.robot.move_to_point(pose=jump_point, t=2.5, timeout=0.5, algorithm='admittance',is_interrupt=False) self.process_except(code) else: if self.resources.mode_real != 0 and self.resources.mode_real != 3: massage_wrench = np.array([0.5, 0.5, 0.5, 0.1, 0.1, 0.1]) code = self.resources.robot.apply_force(wrench=massage_wrench, t=1, algorithm=self.resources.use_algorithm) self.process_except(code) up_target_point = copy.deepcopy(jump_point) if self.resources.choose_task == "ion": up_target_point[3:] = copy.deepcopy(self.back_angle_ion) up_target_point[:3] = jump_point[:3] + self.tf2base([0, 0, -self.resources.robot.arm_state.desired_high], self.back_angle_ion) # else: # up_target_point = copy.deepcopy(jump_point) # up_target_point[3:] = self.resources.robot.arm.level_base # up_target_point[:3] = up_target_point[:3] + self.tf2base([0, 0, -0.10], self.resources.robot.arm.level_base) if self.resources.choose_task == "ion": code = self.resources.robot.move_to_point(pose=up_target_point, t=jump_time*1.5, timeout=0.5, algorithm="positionerSensor", is_interrupt=False,is_switch_controller=True) else: code = self.resources.robot.move_to_point(pose=up_target_point, t=jump_time*1.5, timeout=0.5, algorithm="admittanceZ", is_interrupt=False,is_switch_controller=True) self.process_except(code) code = self.resources.robot.move_to_point(pose=jump_point, t=2, timeout=0.5, algorithm=self.resources.use_algorithm,is_interrupt=False,is_switch_controller=True) self.process_except(code) if self.resources.mode_real != 0 and self.resources.mode_real != 3: code = self.resources.robot.apply_force(wrench=self.resources.robot.arm_state.massage_wrench, t=3, algorithm=self.resources.use_algorithm) self.process_except(code) self.resources.is_jump = False def process_high_safety(self,target_point): # 获取拍照位置 admit_begin_pos = copy.deepcopy(self.resources.cam_position) ground_pos = admit_begin_pos + self.tf2base([0, 0, self.resources.robot.arm.cam_length], self.resources.robot.arm.level_base) self.resources.logger.log_blue(f"ground_pos:{ground_pos}") self.admit_begin_pos = ground_pos + self.tf2base([0, 0, -self.resources.high_st_limit], self.resources.robot.arm.level_base) low_st_limit_pos = ground_pos + self.tf2base([0, 0, -self.resources.low_st_limit], self.resources.robot.arm.level_base) self.resources.logger.log_blue(f"self.admit_begin_pos:{self.admit_begin_pos},{admit_begin_pos + self.tf2base([0, 0, self.resources.robot.arm.cam_length],self.resources.robot.arm.level_base)}") target_point[3:] = self.resources.robot.arm.level_base target_point[:3] = target_point[:3] + self.tf2base([0, 0, -0.15], self.resources.robot.arm.level_base) admit_high = R.from_euler("xyz",self.resources.robot.arm.level_base,degrees=False).as_matrix() @ self.admit_begin_pos target_high = R.from_euler("xyz",self.resources.robot.arm.level_base,degrees=False).as_matrix() @ target_point[:3] target_low = R.from_euler("xyz",self.resources.robot.arm.level_base,degrees=False).as_matrix() @ low_st_limit_pos self.resources.logger.log_blue(f"admit_high:,{admit_high},{target_high},{target_low}") if target_high[2] >= target_low[2]: self.resources.logger.log_error("目标点深度异常") self.resources.send_instruction(instruction={"message": "目标位置深度信息异常
请检测设备情况"}, target="ui_on_message") return False, target_point if target_high[2] >= admit_high[2]: self.resources.logger.log_blue(f"第一个目标点高度{target_high[2]}低于{admit_high[2]},将导纳开始位置置于{self.resources.high_st_limit}") target_high[2] = copy.deepcopy(admit_high[2]) target_point[:3] = R.from_euler("xyz",self.resources.robot.arm.level_base,degrees=False).as_matrix().T @ target_high self.start_time = 10 + np.abs((target_high[2] - admit_high[2])/0.015) else: self.start_time = 10 self.resources.logger.log_blue(f"导纳运动时间为{self.start_time}") return True, target_point def move_to_startpos(self): # 任务执行步骤 # 到达第一个点位上方 # 获取第一组数据 first_command = self.resources.mode_commands_list[0] # 获取第一组数据中的第一个元素（即 tempMassagePoints[0]） first_pos = first_command[0] self.first_bodypart = first_command[8] if first_pos is None: self.resources.logger.log_error("the first position is None!") raise StopWorkflow if self.first_bodypart is None: # 若没有指定bodypart，则默认为back self.first_bodypart = "back" self.resources.logger.log_blue(f"读取第一个点位:{first_pos}") self.target_point_list = [] self.target_point_list.append(np.array(first_pos)) # 给视觉发送xy坐标读取返回的位置姿态 self.resources.logger.log_blue(f"开始发送第一个点位:{self.target_point_list}") self.target_point_list = self.resources.coordinate_tf.pixel_to_robotarm(self.target_point_list,number=self.resources.show_image_num) if self.target_point_list is None: if self.resources.body_part == "belly": self.resources.send_instruction(instruction={"message": "您躺的位置有点不合适呢
请尝试往上躺一点呢"}, target="ui_on_message") else: self.resources.send_instruction(instruction={"message": "您躺的位置有点不合适呢
请尝试往下躺一点呢"}, target="ui_on_message") if self.resources.is_pause: raise PauseWorkflow else: raise StopWorkflow instruction = {"path": os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/Pixel_Visualization_{self.resources.show_image_num}.jpg"} self.resources.logger.log_info(f'展示图片编号:{self.resources.show_image_num}') self.resources.send_instruction(instruction=instruction, target="ui_change_image") task = self.resources.choose_task instruction = f"{task}_introduction" if task in ["shockwave", "finger", "roller", "spheres", "thermotherapy", "ball", "stone","ion", "heat"] else None if instruction: self.resources.send_instruction(instruction=instruction, target="language") else: self.resources.logger.log_error("未知的任务类型") raise StopWorkflow if self.resources.robot.force_sensor: self.resources.robot.force_sensor.stop_background_reading() self.resources.robot.force_sensor = XjcSensor() # 初始化实例化力传感器 time.sleep(0.2) code = self.resources.robot.sensor_set_zero() if code != 0: self.resources.logger.log_error("传感器初始化失败") self.resources.send_instruction(instruction={"message": "传感器初始化失败"}, target="ui_on_message") if self.resources.is_pause: raise PauseWorkflow else: raise StopWorkflow code = self.resources.robot.sensor_enable() if code != 0: self.resources.logger.log_error("传感器初始化失败") self.resources.send_instruction(instruction={"message": "传感器初始化失败"}, target="ui_on_message") if self.resources.is_pause: raise PauseWorkflow else: raise StopWorkflow #启动机械臂，切换按摩头palyload task = self.resources.choose_task current_head = f"{task}_head" if task in ["shockwave", "finger", "roller", "spheres","thermotherapy", "ball", "stone","ion", "heat"] else None if self.resources.choose_task == "thermotherapy": self.resources.logger.log_info('启动热疗按摩') self.resources.robot.thermotherapy.set_working_status(cur_level=self.resources.gear, heat_level=self.resources.temperature, vib_level=self.resources.shake) elif self.resources.choose_task == "stone": self.resources.logger.log_info('启动砭石按摩') self.resources.robot.stone.set_working_status(heat_level=self.resources.temperature, speed_level=self.resources.speed, rotate_direction=self.resources.direction) elif self.resources.choose_task == "heat": self.resources.logger.log_info('启动能量按摩') self.resources.robot.heat.set_working_status(heat_level=self.resources.temperature) if current_head: self.resources.robot.switch_payload(current_head) else: self.resources.logger.log_error("未知的按摩头类型") raise StopWorkflow #TODO 根据不同的任务选择不同的pose time.sleep(0.1) self.resources.logger.log_yellow("运动到伺服启动位置") self.resources.robot.is_waitting = True self.resources.robot.arm.disable_servo() time.sleep(0.5) self.resources.logger.log_blue(f"接受的第一个点位:{self.target_point_list}") target_point = copy.deepcopy(self.target_point_list[0]) code,target_point = self.process_high_safety(target_point) if not code: raise StopWorkflow self.resources.logger.log_blue(f"move_to_points: {target_point}") code = self.resources.robot.set_position(pose=target_point,is_wait=True) #先到达第一个点位上方 if code == -1: self.resources.logger.log_error("运动伺服启动位置失败") self.resources.stop_event.set() raise EmergencyStopWorkflow time.sleep(3.5) self.resources.robot.is_waitting = False self.resources.robot.start() time.sleep(0.5) def wait_queue(self): #等待规划生成线程完成并预处理，将plantask赋值给motiontask，并且置为None，发送信息到前端 if self.resources.insert_queue_thread is not None: self.resources.insert_queue_thread.join() self.resources.insert_queue_thread = None self.resources.is_skip = False #重置跳过标志位 self.resources.generate_thread.join() self.resources.generate_thread = None instruction = {"path": os.path.dirname(os.path.abspath(__file__)) + f"/aucpuncture2point/configs/using_img/Pixel_Visualization_{self.resources.show_image_num}.jpg"} self.resources.logger.log_blue(f"展示图片:{self.resources.show_image_num}") self.resources.send_instruction(instruction=instruction, target="ui_change_image") self.resources.motion_task = copy.deepcopy(self.resources.plan_task) #将规划任务赋给运动任务执行，更新 self.resources.motion_count += 1 self.resources.move_count += 1 self.resources.logger.log_blue(f"move_count:{self.resources.move_count},{self.resources.current_queue_num},{self.resources.show_image_num},{self.resources.insert_len}") instruction = {} instruction["start_pos"] = self.resources.motion_task[9] instruction["end_pos"] = self.resources.motion_task[10] instruction["massage_path"] = self.resources.motion_task[5] self.resources.send_instruction(instruction, target="ui_update_massage_status") self.resources.plan_task = None self.resources.target_points = copy.deepcopy(self.resources.next_points) #锁定姿态 num_points = len(self.resources.target_points) task = self.resources.choose_task motion = self.resources.motion_task[8] if task == "ion": angle = self.back_angle_ion else: angle = self.back_angle if motion == "leg" or (task == "shockwave" and motion != "shoulder") or task == "ion": for i in range(num_points): self.resources.target_points[i][3:] = angle if (self.resources.motion_task[6] is None or self.resources.motion_task[6] == 0): if self.resources.motion_task[5] != "point" and self.resources.motion_task[5] != "point_rub": self.resources.motion_task[6] = self.resources.path_length/self.resources.path_velocity else: self.resources.logger.log_info("未设置点摁的按摩时间") self.resources.motion_task[6] = 20 # 当未设置点摁的按摩时间时，默认点摁按摩时间为20s self.resources.motion_task[6] = max(0.5, min(450, self.resources.motion_task[6])) def execute_movement(self): #执行motiontask任务 self.resources.logger.log_info(f"{self.resources.motion_task} 按摩中...") if self.resources.is_jump: self.process_jump_deal(jump_point=copy.deepcopy(self.resources.target_points[0])) self.resources.robot.is_execute = True if self.resources.motion_task[7] is not None: self.resources.robot.arm_state.massage_wrench[2] = self.resources.motion_task[7] code = self.resources.robot.apply_force(wrench=self.resources.robot.arm_state.massage_wrench, t=1, algorithm=self.resources.use_algorithm) self.process_except(code) if self.resources.skip_event.is_set() and self.resources.robot.skip_envent.is_set(): self.resources.logger.log_blue("检测到跳过，准备进入跳过流程") self.resources.robot.is_execute = False return True if self.resources.choose_task == "shockwave": self.resources.robot.shockwave.on() # 开启冲击波 self.resources.robot.shockwave.p_set(level = self.resources.press) self.resources.robot.shockwave.f_set(level = self.resources.frequency) if self.resources.motion_task[5] == "point" or self.resources.motion_task[5] == "point_rub": use_algorithm = self.resources.use_algorithm if self.resources.choose_task != "ion" and self.resources.motion_task[5] == "point": self.resources.use_algorithm = "hybridPid" print("self.resources.use_algorithm:",self.resources.use_algorithm) code = self.resources.robot.move_to_point(pose=self.resources.target_points, t=self.resources.motion_task[6], timeout=0.5, interpolation="circle", algorithm=self.resources.use_algorithm,is_interrupt=False,is_switch_controller = self.resources.is_switch_controller) self.resources.use_algorithm = use_algorithm else: code = self.resources.robot.move_to_point(pose=self.resources.target_points, t=self.resources.motion_task[6], timeout=0.5, interpolation="cloud_point", algorithm=self.resources.use_algorithm,is_interrupt=False,is_switch_controller = self.resources.is_switch_controller) # self.process_except(code) if self.resources.skip_event.is_set() and self.resources.robot.skip_envent.is_set(): self.resources.logger.log_blue("检测到跳过，准备进入跳过流程") self.resources.robot.is_execute = False return True #准备处理之前插入 if self.resources.plan_task is not None: if self.resources.plan_task[0] is not None and not np.allclose(self.resources.plan_task[0], self.resources.motion_task[1], atol=0.1): # 跳跃准备 self.resources.is_jump = True if self.resources.motion_task[11] is not None: self.resources.jump_mode = copy.deepcopy(self.resources.motion_task[11]) else: self.resources.jump_mode = copy.deepcopy(self.resources.jump_mode_default) self.resources.jump_length = np.linalg.norm(self.resources.plan_task[0] - self.resources.motion_task[1]) self.resources.robot.is_execute = False return False def process_queue(self):# 从队列中获取任务赋值给planTASK用于生成线程规划 # 获取任务信息 self.resources.plan_task = self.resources.mode_commands_list.popleft() if self.resources.insert_len > 0: self.resources.insert_len -= 1 else: self.resources.current_queue_num += 1 # 队列取出来第几个任务 self.resources.logger.log_info(f"plan_task:{self.resources.plan_task}") #startPos,endPos,pathWidth,pathCircles,pathDir,pathMode if self.resources.plan_task[0].tolist() is None: if self.resources.plan_task[5] == "point" or self.resources.plan_task[5] == "point_rub": self.resources.plan_task[0] = copy.deepcopy(self.resources.plan_task[1]) # 赋值为上一步的结束位置,更新任务队列 else: self.resources.plan_task[0] = copy.deepcopy(self.resources.motion_task[1]) # 更新任务队列 self.resources.logger.log_blue(f"修正 startPos:{self.resources.plan_task[0]}") # 启动新线程 self.resources.logger.log_info(f"开始规划 {self.resources.plan_task} 路线") self.resources.generate_thread = threading.Thread(target=self.resources.generate_next_points, args=(self.resources.plan_task[0],self.resources.plan_task[1],self.resources.plan_task[2],self.resources.plan_task[3]),kwargs={'direction':self.resources.plan_task[4],'path':self.resources.plan_task[5]}) self.resources.generate_thread.start() def first_touch(self): self.resources.logger.log_info("预处理-完成") self.resources.send_instruction(instruction="skin_touch",target="language") if self.resources.choose_task == "shockwave" and self.first_bodypart != "shoulder": # 修正冲击波姿态 self.target_point_list[0][3:] = copy.deepcopy(self.back_angle) if self.resources.choose_task == "ion": self.target_point_list[0][3:] = copy.deepcopy(self.back_angle_ion) touch_pos = copy.deepcopy(self.target_point_list[0]) touch_pos[:3] = touch_pos[:3] + self.tf2base([0, 0, 0.03], self.resources.robot.arm.level_base) self.resources.logger.log_blue(f"move_to_points: {touch_pos}") if self.resources.choose_task == "shockwave" or self.resources.choose_task == "ion": code = self.resources.robot.move_to_point(pose=touch_pos, t=self.start_time, timeout=0.5, algorithm="admithybrid",is_interrupt=False,is_switch_controller = True) else: code = self.resources.robot.move_to_point(pose=touch_pos, t=self.start_time, timeout=0.5, algorithm="admittance",is_interrupt=False,is_switch_controller = True) self.process_except(code) if self.resources.mode_real != 0 and self.resources.mode_real != 3:# 开启力位混合 code = self.resources.robot.apply_force(wrench=self.resources.robot.arm_state.massage_wrench, t=5, algorithm=self.resources.use_algorithm) self.process_except(code) self.resources.is_first_move = False def skip_process(self): self.resources.logger.log_info(f"进入跳过流程,skip_pos:{self.resources.robot.skip_pos}") self.resources.skip_event.clear() self.resources.robot.skip_envent.clear() if self.resources.mode_real != 0 and self.resources.mode_real != 3: massage_wrench = np.array([0, 0, 0, 0, 0, 0]) code = self.resources.robot.apply_force(wrench=massage_wrench, t=1, algorithm=self.resources.use_algorithm) self.process_except(code) if self.resources.choose_task == "shockwave": self.resources.robot.shockwave.off() if self.resources.choose_task == "shockwave": code = self.resources.robot.move_to_point(pose=self.resources.robot.skip_pos, t=5, timeout=0.5, algorithm="admithybrid",is_interrupt=False,is_switch_controller = True) else: code = self.resources.robot.move_to_point(pose=self.resources.robot.skip_pos, t=5, timeout=0.5, algorithm="admittance",is_interrupt=False,is_switch_controller = True) self.process_except(code) #准备处理之前插入 if self.resources.plan_task is not None: # 准备将六维坐标转换为二维坐标方便在像素坐标系中处理 # rotation_matrix = R.from_euler(self.turn_2D, "xyz", degrees=False).as_matrix() rotation_matrix = R.from_euler("xyz", self.turn_2D, degrees=False).as_matrix() # 构建线性方程组：A @ P + b = Q # 所以 A = (Q2 - Q1) @ inv(P2 - P1), b = Q1 - A @ P1 # 原空间 startPos = np.array((rotation_matrix @ self.resources.target_points[0][:3])[:2]) endPos = np.array((rotation_matrix @ self.resources.target_points[-1][:3])[:2]) # 目标空间 targetStart = np.array(copy.deepcopy(self.resources.motion_task[0])) targetEnd = np.array(copy.deepcopy(self.resources.motion_task[1])) # 计算向量差 dP = startPos - endPos # 差向量 (原空间) dQ = targetStart - targetEnd # 差向量 (目标空间) if (np.abs(dP) < 0.001).all() or (np.abs(dQ) < 0.001).all(): skipPos2D = copy.deepcopy(self.resources.motion_task[0]) else: # 转换成列向量来构建仿射矩阵 A A = np.outer(dQ, dP) / np.dot(dP, dP) # 求 b b = targetStart - A @ startPos # 要转换的第三个点 skipPos = np.array((rotation_matrix @ self.resources.robot.skip_pos[:3])[:2]) # 计算在目标空间中的对应点 skipPos2D = A @ skipPos + b self.resources.logger.log_info(f"skipPos在二维像素坐标系中的对应点: {skipPos2D}") self.resources.is_jump = True self.resources.jump_length = np.linalg.norm(self.resources.plan_task[0] - skipPos2D) def tasks_queue_execution(self): # 开始按摩 self.resources.logger.log_info(f"切换控制方式: {self.resources.is_switch_controller}") self.resources.logger.log_info(f"use_algorithm: {self.resources.use_algorithm}") self.resources.logger.log_info(f"任务长度: {self.resources.massage_task_num}") # 在单独的线程中启动 update_time 方法 self.resources.update_thread = threading.Thread(target=self.resources.update_time) self.resources.update_thread.start() self.resources.is_pause = False instruction = { "is_pause": False, } self.resources.send_instruction(instruction, "ui_update_massage_status") # 获取当前时间，并格式化为字符串（例如：2025-04-01_10-00-00.json） current_time = datetime.datetime.now().strftime("%Y-%m-%d_%H%M%S") filename = f"{current_time}_{self.resources.choose_task}_{self.resources.body_part}_{self.resources.selected_plan}.json" while True: while self.resources.mode_commands_list: # 等待生成轨迹点的线程完成，获取上一步的target_points if self.resources.is_first_move == False: self.wait_queue() #等待生成线程完成并预处理 self.process_queue() if self.resources.is_first_move == False:# 执行上一步 is_skip = self.execute_movement() #按顺序执行已生成的command self.write_to_json(filename=filename) if self.resources.is_finish == True and self.resources.loops == 0: self.resources.logger.log_blue(f"时间已到，完成按摩") break if is_skip: self.skip_process() else: self.write_to_json(filename=filename) else: self.first_touch() #进行第一次接触 if self.resources.is_finish == True and self.resources.loops == 0: self.resources.logger.log_blue(f"时间已到，完成按摩") break self.wait_queue() # 执行最后一步 is_skip = self.execute_movement() self.write_to_json(filename=filename) if self.resources.is_finish == True and self.resources.loops == 0: self.resources.logger.log_blue(f"时间已到，完成按摩") break if is_skip: self.skip_process() else: self.write_to_json(filename=filename) if self.resources.mode_commands_list: self.resources.logger.log_blue(f"仍存在未执行完毕的队列：{self.resources.mode_commands_list}") else: if self.resources.is_finish == False and self.resources.loops == 0: self.resources.logger.log_blue(f"队列执行完毕但时间未到,复制default给队列执行:{self.resources.mode_commands_list_default}") self.resources.mode_commands_list = copy.deepcopy(self.resources.mode_commands_list_default) self.resources.current_queue_num = -2 else: self.resources.logger.log_blue(f"时间已到，完成按摩") self.resources.is_finish = True #按摩完成标志 break if self.resources.generate_thread is None and self.resources.insert_queue_thread is None: self.process_queue() if self.resources.plan_task is not None: if self.resources.plan_task[0] is not None and not np.allclose(self.resources.plan_task[0], self.resources.motion_task[1], atol=0.1): # 跳跃准备 self.resources.is_jump = True self.resources.jump_length = np.linalg.norm(self.resources.plan_task[0] - self.resources.motion_task[1]) self.resources.logger.log_yellow("重新进入循环执行队列") if self.resources.is_skip == False:#完成按摩 massage_wrench = np.array([0, 0, 0, 0, 0, 0]) code = self.resources.robot.apply_force(wrench=massage_wrench, t=1, algorithm= "hybrid") self.process_except(code) if self.resources.choose_task == "shockwave": self.resources.robot.shockwave.off() # 关闭冲击波 up_target_point = copy.deepcopy(self.resources.target_points[-1]) up_target_point[:3] = up_target_point[:3] + self.tf2base([0, 0, -0.10], up_target_point[3:]) self.resources.logger.log_blue(f"move_to_points: {up_target_point}") if self.resources.choose_task == "shockwave" or self.resources.choose_task == "ion": code = self.resources.robot.move_to_point(pose=up_target_point, t=1.5, timeout=0.5, algorithm="admithybrid", is_interrupt=False,is_switch_controller = True) else: code = self.resources.robot.move_to_point(pose=up_target_point, t=1.5, timeout=0.5, algorithm="admittance", is_interrupt=False,is_switch_controller = True) self.process_except(code) self.resources.is_skip = False instruction = {"progress": "100"} self.resources.send_instruction(instruction, "ui_update_massage_status") self.resources.init_queue_para() #初始化队列参数 class Thermotherapy_Task(TaskBase): def __init__(self, resources: SharedResources): super(Thermotherapy_Task, self).__init__(resources) self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Thermotherapy_Task") self.move_to_startpos() self.resources.logger.log_info('启动热疗按摩') # self.resources.robot.thermotherapy.set_working_status(cur_level=self.resources.gear, heat_level=self.resources.temperature, vib_level=self.resources.shake) self.tasks_queue_execution() # 按照任务队列执行任务 return "stop" except PauseWorkflow: self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") time.sleep(0.5) return "emergency" class Shockwave_Task(TaskBase): def __init__(self, resources: SharedResources): super(Shockwave_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.max_radius_st = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Shockwave_Task") self.move_to_startpos() self.tasks_queue_execution() # 按照任务队列执行任务 return "stop" except PauseWorkflow: if self.resources.robot.shockwave is not None: self.resources.robot.shockwave.off() # 关闭冲击波 self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: if self.resources.robot.shockwave is not None: self.resources.robot.shockwave.off() # 关闭冲击波 self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: if self.resources.robot.shockwave is not None: self.resources.robot.shockwave.off() # 关闭冲击波 self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") return "emergency" class Ball_Task(TaskBase): def __init__(self, resources: SharedResources): super(Ball_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.max_radius_st = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Ball_Task") self.move_to_startpos() self.tasks_queue_execution() # 按照任务队列执行任务 return "stop" except PauseWorkflow: self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") time.sleep(0.5) return "emergency" class Stone_Task(TaskBase): def __init__(self, resources: SharedResources): super(Stone_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.max_radius_st = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Stone_Task") self.move_to_startpos() self.resources.logger.log_info('启动砭石按摩') self.resources.robot.stone.set_working_status(heat_level=self.resources.temperature, speed_level=self.resources.speed , rotate_direction=self.resources.direction) self.tasks_queue_execution() # 按照任务队列执行任务 return "stop" except PauseWorkflow: self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") time.sleep(0.5) return "emergency" class Finger_Task(TaskBase): def __init__(self, resources: SharedResources): super(Finger_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.max_radius_st = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Finger_Task") self.move_to_startpos() self.tasks_queue_execution() # 按照任务队列执行任务s return "stop" except PauseWorkflow: time.sleep(0.5) self.resources.motion_task = None self.resources.plan_task = None return "pause" except StopWorkflow: self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") return "emergency" class Heat_Task(TaskBase): def __init__(self, resources: SharedResources): super(Heat_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.max_radius_st = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Heat_Task") self.move_to_startpos() self.tasks_queue_execution() # 按照任务队列执行任务s if self.resources.robot.heat is not None: self.resources.robot.heat.power_off() return "stop" except PauseWorkflow: if self.resources.robot.heat is not None: self.resources.robot.heat.power_off() self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: if self.resources.robot.heat is not None: self.resources.robot.heat.power_off() self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: if self.resources.robot.heat is not None: self.resources.robot.heat.power_off() self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") time.sleep(0.5) return "emergency" class Roller_Task(TaskBase): def __init__(self, resources: SharedResources): super(Roller_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Roller_Task") self.move_to_startpos() self.tasks_queue_execution() # 按照任务队列执行任务s return "stop" except PauseWorkflow: self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") time.sleep(0.5) return "emergency" class Spheres_Task(TaskBase): def __init__(self, resources: SharedResources): super(Spheres_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 def execute(self, userdata): try: self.resources.logger.log_info("Executing Spheres_Task") self.move_to_startpos() self.tasks_queue_execution() # 按照任务队列执行任务s return "stop" except PauseWorkflow: self.resources.motion_task = None self.resources.plan_task = None time.sleep(0.5) return "pause" except StopWorkflow: self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.init_queue_para() self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") time.sleep(0.5) return "emergency" class Ion_Task(TaskBase): def __init__(self, resources: SharedResources): super(Ion_Task, self).__init__(resources) # 存储测试使用时间 self.time = 0 self.last_point = np.array([]) # 储存上个路径最后一个点 self.stop_ion = threading.Event() self.stop_ion.clear() self.ion_thread = None def ion_update_dis(self): try: countDanger = 0 while not self.stop_ion.is_set(): # 获取数据并进行处理 # 处理烫伤风险 if self.resources.robot.ion.is_reconnect == False: self.resources.robot.arm_state.positionerSensorData = self.resources.robot.ion.ver_dis_kalman if self.resources.robot.arm_state.positionerSensorData < 0.015 and self.resources.robot.arm_state.positionerSensorData > 0: countDanger += 1 else: countDanger = 0 else: self.resources.robot.arm_state.positionerSensorData = 0 countDanger = 0 if countDanger > 10: # 当风险超过阈值时发送警告 self.resources.send_instruction(instruction={"message": "检测到烫伤的风险
机器人停止按摩"}, target="ui_on_message") self.stop_ion.set() self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() # 确保异常发生时关闭设备 self.resources.robot.ion.stop_monitoring() self.resources.stop_event.set() self.resources.robot.interrupt_event.set() break time.sleep(0.05) # 延时防止CPU占用过高 except Exception as e: # 捕获所有其他异常 print(f"捕获到异常: {e}") self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() # 确保异常发生时关闭设备 self.resources.robot.ion.stop_monitoring() self.stop_ion.set() self.resources.stop_event.set() self.resources.robot.interrupt_event.set() finally: # 无论是否发生异常，都会执行的清理操作 print("退出 ion_update_dis 线程") self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() # 确保设备关闭 self.resources.robot.ion.stop_monitoring() def execute(self, userdata): try: self.resources.logger.log_info("Executing Stone_Task") self.move_to_startpos() # self.resources.robot.ion.power_on() self.resources.robot.ion.start_monitoring(0.05) self.resources.robot.ion.set_working_status(goal_state=self.resources.temperature) self.ion_thread = threading.Thread(target=self.ion_update_dis) self.ion_thread.start() self.tasks_queue_execution() # 按照任务队列执行任务s self.stop_ion.set() self.ion_thread.join() self.ion_thread = None self.stop_ion.clear() self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() return "stop" except PauseWorkflow: self.resources.motion_task = None self.resources.plan_task = None self.stop_ion.set() # 设置事件，通知线程停止 # 等待线程结束 if self.ion_thread is not None: self.ion_thread.join() self.ion_thread = None self.stop_ion.clear() self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() time.sleep(0.5) time.sleep(0.5) return "pause" except StopWorkflow: self.stop_ion.set() # 设置事件，通知线程停止 # 等待线程结束 if self.ion_thread is not None: self.ion_thread.join() self.ion_thread = None self.stop_ion.clear() self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() self.resources.init_queue_para() time.sleep(0.5) return "stop" except EmergencyStopWorkflow: self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.send_instruction({"progress": "0"}, "ui_update_massage_status") self.stop_ion.set() # 设置事件，通知线程停止 # 等待线程结束 if self.ion_thread is not None: self.ion_thread.join() self.ion_thread = None self.stop_ion.clear() self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() self.resources.init_queue_para() time.sleep(0.5) return "emergency" class Idle(smach.State): def __init__(self, resources: SharedResources): smach.State.__init__(self, outcomes=["to_manual","to_smart"]) """空闲状态下不进行移动，可以停在任意位置，等待开始信号""" self.resources = resources def execute(self, userdata): self.resources.logger.log_info("Executing state idle") self.resources.logger.log_info("等待开始") self.resources.stop_event.clear() self.resources.begin_event.wait() self.resources.begin_event.clear() time.sleep(0.5) self.resources.logger.log_info( "now_process_task:" + str(self.resources.choose_task) ) self.resources.send_instruction({"is_massaging": True}, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") if self.resources.use_mode == "manual": return "to_manual" else: return "to_smart" class Stop(smach.State): def __init__(self, resources: SharedResources): """停止状态下自动返回初始位置，然后进入空闲状态""" smach.State.__init__(self, outcomes=["to_idle", "emergency"]) self.resources = resources def execute(self, userdata): self.resources.logger.log_info("Executing state stop") # 获取当前时间并格式化为指定格式 current_time = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") # 构建按摩头数据 instruction = { "stop_time": current_time, "head_type": self.resources.choose_task, "body_part": self.resources.body_part } self.resources.send_instruction(instruction, target="iot_massage_head") self.resources.is_finish = True self.resources.send_instruction("run_stop", "language") if self.resources.tool_cam: self.resources.tool_cam.stop() self.resources.get_picture = True self.resources.cal_acu = False self.resources.manual_start = False self.resources.selected_plan = None if self.resources.update_thread is not None: self.resources.update_thread.join() self.resources.update_thread = None self.resources.is_acupoint = False #重置穴位展示标示位 self.resources.progress = 0 self.resources.current_part = None self.resources.task_time = 0 self.resources.current_time = 0 self.resources.loops = 0 self.resources.start_time = 0 self.resources.body_part = None self.resources.mode_real = 1 self.resources.use_mode = "smart" self.resources.show_image_num = 0 self.resources.massage_task_num = 0 self.resources.move_count = 0 self.resources.motion_count = 0 self.resources.insert_len = 0 self.resources.motion_task = None self.resources.plan_task = None self.resources.mode_commands_list = deque([]) self.resources.mode_commands_list_default = deque([]) self.resources.is_first_move = True self.resources.is_jump = False self.resources.is_skip = False self.resources.is_insert = False self.resources.current_queue_num = -2 self.resources.target_points = None self.resources.robot.arm_state.massage_wrench = np.zeros(6) self.resources.temperature = self.resources.temperature_default self.resources.gear = self.resources.gear_default self.resources.shake = self.resources.shake_default self.resources.press = self.resources.press_default self.resources.frequency = self.resources.frequency_default self.resources.speed = self.resources.speed_default self.resources.direction = self.resources.direction_default if self.resources.robot.controller_manager.current_controller.name == "hybrid": code = self.resources.robot.apply_force( wrench=np.zeros(6), t=1, algorithm="hybrid" ) if code == 3: return "emergency" elif self.resources.robot.controller_manager.current_controller.name == "hybridPid": code = self.resources.robot.apply_force( wrench=np.zeros(6), t=1, algorithm="hybridPid" ) if code == 3: return "emergency" elif self.resources.robot.controller_manager.current_controller.name == "hybridAdmit": code = self.resources.robot.apply_force( wrench=np.zeros(6), t=1, algorithm="hybridAdmit" ) if code == 3: return "emergency" self.resources.robot.is_waitting = True time.sleep(1) self.resources.robot.arm.disable_servo() self.resources.robot.arm.disable_servo() self.resources.robot.stop_thread() if self.resources.next_points is not None: time.sleep(1) position,quat_rot = self.resources.robot.arm.get_arm_position() time.sleep(0.2) target_point = np.zeros(6) target_point[:3] = copy.deepcopy(position) target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False)) # target_point[:3] = copy.deepcopy(self.resources.robot.arm_state.arm_position_command) # target_point[3:] = copy.deepcopy(R.from_quat(self.resources.robot.arm_state.arm_orientation_command).as_euler('xyz',degrees=False)) # target_point[3:] = [150 * (np.pi / 180), 0, 180 * (np.pi / 180)] target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.10], self.resources.robot.arm.level_base) self.resources.logger.log_blue(f"set_position: {target_point},{position}") code = self.resources.robot.set_position(pose=target_point,is_wait=True) self.resources.next_points = None time.sleep(1) pose = copy.deepcopy(self.resources.robot.arm.standby_pos) self.resources.logger.log_yellow("self.resources.robot.arm.disable_servo()") time.sleep(1) if not self.resources.is_pause: # self.resources.robot.arm.move_joint(pose) code = self.resources.robot.set_joint(pose) if code == -1: self.resources.logger.log_error(f"运动到pose:{pose}位置失败") return "emergency" pose = copy.deepcopy(self.resources.robot.arm.init_pos) time.sleep(0.1) if not self.resources.is_pause: # self.resources.robot.arm.move_joint(pose) code = self.resources.robot.set_joint(pose) if code == -1: self.resources.logger.log_error(f"运动到pose:{pose}位置失败") return "emergency" time.sleep(1) if self.resources.choose_task == "thermotherapy": self.resources.logger.log_info('关闭热疗按摩') if self.resources.robot.thermotherapy is not None: for _ in range(3): self.resources.robot.thermotherapy.stop_process(); time.sleep(0.5) elif self.resources.choose_task == "shockwave": self.resources.logger.log_info('关闭点阵按摩') if self.resources.robot.shockwave is not None: for _ in range(3): self.resources.robot.shockwave.off(); time.sleep(0.5) elif self.resources.choose_task == "stone": self.resources.logger.log_info('关闭砭石按摩') if self.resources.robot.stone is not None: for _ in range(3): self.resources.robot.stone.power_off(); time.sleep(0.5) elif self.resources.choose_task == "heat": self.resources.logger.log_info('关闭能量按摩') if self.resources.robot.heat is not None: for _ in range(3): self.resources.robot.heat.power_off(); time.sleep(0.5) elif self.resources.choose_task == "ion": self.resources.logger.log_info('关闭负离子按摩') if self.resources.robot.ion is not None: for _ in range(3): self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() time.sleep(0.5) self.resources.choose_task = None self.resources.robot.thermotherapy = None self.resources.robot.shockwave = None self.resources.robot.stone = None self.resources.robot.heat = None del self.resources.robot.ion self.resources.robot.ion = None if self.resources.update_massageHead_temper_thread is not None: self.resources.update_massageHead_temper_thread.join() self.resources.update_massageHead_temper_thread = None self.resources.temper_head = None self.resources.robot.arm_state.desired_high = self.resources.desired_high_default self.resources.robot.is_waitting = False self.resources.send_instruction("finish", "language") self.resources.robot.stop() self.resources.stop_event.clear() self.resources.pause_event.clear() self.resources.skip_event.clear() self.resources.is_pause = False self.resources.is_acupoint = False instruction = {} instruction["is_massaging"] = (False) instruction["is_acupoint"] = (False) if self.resources.manual_stage != 0: self.resources.manual_stage = 0 instruction["manual_stage"] = self.resources.manual_stage self.resources.send_instruction(instruction, "ui_update_massage_status") self.resources.send_instruction({"is_pause": False}, "ui_update_massage_status") self.resources.logger.log_info("Massage stopped!") return "to_idle" class Pause(smach.State): def __init__(self, resources: SharedResources): """停止状态下自动返回初始位置，然后进入空闲状态""" smach.State.__init__(self, outcomes=["to_smart", "to_manual", "stop", "emergency"]) self.resources = resources def execute(self, userdata): self.resources.logger.log_info("Executing state pause") self.resources.is_finish = True self.resources.send_instruction(instruction="pause_massage", target="language") print("zhixing dao duilie de di jige renwu:",self.resources.current_queue_num) print("renwuduilie:",self.resources.mode_commands_list) self.resources.is_acupoint = False if self.resources.update_thread is not None: self.resources.update_thread.join() self.resources.update_thread = None self.resources.send_instruction("run_pause", "language") if self.resources.tool_cam: self.resources.tool_cam.stop() self.resources.get_picture = True self.resources.cal_acu = False self.resources.manual_start = False self.resources.is_first_move = True self.resources.is_jump = False self.resources.is_skip = False self.resources.is_insert = False self.resources.manual_start = False self.resources.acupuncture_dict = None self.resources.manual_stage = 1 self.resources.insert_len = 0 self.resources.motion_task = None self.resources.plan_task = None self.resources.mode_commands_list = deque([]) self.resources.target_points = None if self.resources.robot.controller_manager.current_controller.name == "hybrid": code = self.resources.robot.apply_force( wrench=np.zeros(6), t=0, algorithm="hybrid" ) if code == 3: return "emergency" elif self.resources.robot.controller_manager.current_controller.name == "hybridPid": code = self.resources.robot.apply_force( wrench=np.zeros(6), t=0, algorithm="hybridPid" ) if code == 3: return "emergency" elif self.resources.robot.controller_manager.current_controller.name == "hybridAdmit": code = self.resources.robot.apply_force( wrench=np.zeros(6), t=1, algorithm="hybridAdmit" ) if code == 3: return "emergency" self.resources.robot.is_waitting = True time.sleep(1) self.resources.robot.arm.disable_servo() self.resources.robot.stop_thread() if self.resources.next_points is not None: time.sleep(1) position,quat_rot = self.resources.robot.arm.get_arm_position() time.sleep(0.2) target_point = np.zeros(6) target_point[:3] = copy.deepcopy(position) target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False)) # target_point[:3] = copy.deepcopy(self.resources.robot.arm_state.arm_position_command) # target_point[3:] = copy.deepcopy(R.from_quat(self.resources.robot.arm_state.arm_orientation_command).as_euler('xyz',degrees=False)) # target_point[3:] = [150 * (np.pi / 180), 0, 180 * (np.pi / 180)] target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.15], self.resources.robot.arm.level_base) self.resources.logger.log_blue(f"set_position: {target_point}") code = self.resources.robot.set_position(pose=target_point,is_wait=True) self.resources.next_points = None time.sleep(1) pose = copy.deepcopy(self.resources.robot.arm.standby_pos) time.sleep(0.1) # self.resources.robot.arm.move_joint(pose) code = self.resources.robot.set_joint(pose) if code == -1: self.resources.logger.log_error(f"运动到pose:{pose}位置失败") return "emergency" pose = copy.deepcopy(self.resources.robot.arm.init_pos) time.sleep(0.1) # self.resources.robot.arm.`move_joint`(pose) code = self.resources.robot.set_joint(pose) if code == -1: self.resources.logger.log_error(f"运动到pose:{pose}位置失败") return "emergency" time.sleep(1) if self.resources.choose_task == "thermotherapy": self.resources.logger.log_info('关闭热疗按摩') if self.resources.robot.thermotherapy is not None: for _ in range(3): self.resources.robot.thermotherapy.stop_process(); time.sleep(0.5) elif self.resources.choose_task == "shockwave": self.resources.logger.log_info('关闭点阵按摩') if self.resources.robot.shockwave is not None: for _ in range(3): self.resources.robot.shockwave.off(); time.sleep(0.5) elif self.resources.choose_task == "stone": self.resources.logger.log_info('关闭砭石按摩') if self.resources.robot.stone is not None: for _ in range(3): self.resources.robot.stone.power_off(); time.sleep(0.5) elif self.resources.choose_task == "ion": self.resources.logger.log_info('关闭负离子按摩') if self.resources.robot.ion is not None: for _ in range(3): self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() time.sleep(0.5) self.resources.robot.thermotherapy = None self.resources.robot.shockwave = None self.resources.robot.stone = None self.resources.robot.heat = None del self.resources.robot.ion self.resources.robot.ion = None self.resources.robot.arm_state.desired_high = self.resources.desired_high_default self.resources.robot.is_waitting = False self.resources.send_instruction("pause", "language") self.resources.robot.stop() self.resources.is_pause = True self.resources.send_instruction({"is_pause": True}, "ui_update_massage_status") self.resources.send_instruction({"is_massaging": False}, "ui_update_massage_status") self.resources.logger.log_info("Massage pause!") self.resources.pause_event.clear() self.resources.skip_event.clear() while True: if self.resources.stop_event.is_set(): return "stop" print("now_begin_envent:",self.resources.begin_event.is_set()) if self.resources.begin_event.is_set(): self.resources.logger.log_info("From state pause to restart manual or cv") self.resources.begin_event.clear() time.sleep(0.5) # self.resources.robot.start() self.resources.logger.log_info( "now_process_task:" + str(self.resources.choose_task) ) self.resources.send_instruction({"is_massaging": True}, "ui_update_massage_status") self.resources.send_instruction(instruction="pause_resume", target="language") if self.resources.use_mode == "manual": return "to_manual" else: self.resources.manual_stage = 1 self.resources.send_instruction({"manual_stage": self.resources.manual_stage}, "ui_update_massage_status") return "to_smart" time.sleep(0.5) class EmergencyExitState(smach.State): def __init__(self, resources: SharedResources): smach.State.__init__(self, outcomes=["emergency_exit"]) self.resources = resources def execute(self, userdata): instruction = { "message": "紧急停止", } self.resources.send_instruction(instruction, "ui_on_message") instruction = { "is_massaging": False, "is_pause": False, "massage_service_started": False, "progress": "0", "is_acupoint": False, } if self.resources.manual_stage != 0: self.resources.manual_stage = 0 self.resources.is_finish = True instruction["manual_stage"] = self.resources.manual_stage if self.resources.choose_task == "thermotherapy": self.resources.logger.log_info('关闭热疗按摩') if self.resources.robot.thermotherapy is not None: for _ in range(3): self.resources.robot.thermotherapy.stop_process(); time.sleep(0.5) elif self.resources.choose_task == "shockwave": self.resources.logger.log_info('关闭点阵按摩') if self.resources.robot.shockwave is not None: for _ in range(3): self.resources.robot.shockwave.off(); time.sleep(0.5) elif self.resources.choose_task == "stone": self.resources.logger.log_info('关闭砭石按摩') if self.resources.robot.stone is not None: for _ in range(3): self.resources.robot.stone.power_off(); time.sleep(0.5) elif self.resources.choose_task == "heat": self.resources.logger.log_info('关闭能量按摩') if self.resources.robot.heat is not None: for _ in range(3): self.resources.robot.heat.power_off(); time.sleep(0.5) elif self.resources.choose_task == "ion": self.resources.logger.log_info('关闭负离子按摩') if self.resources.robot.ion is not None: for _ in range(3): self.resources.robot.arm_state.positionerSensorData = 0 self.resources.robot.ion.power_off() self.resources.robot.ion.stop_monitoring() time.sleep(0.5) if self.resources.update_thread is not None: self.resources.update_thread.join() self.resources.update_thread = None if self.resources.tool_cam: self.resources.tool_cam.stop() self.resources.task_time = 0 self.resources.current_time = 0 self.resources.loops = 0 self.resources.get_picture = True self.resources.cal_acu = False self.resources.manual_start = False self.resources.selected_plan = None self.resources.show_image_num = 0 self.resources.massage_task_num = 0 self.resources.move_count = 0 self.resources.motion_count = 0 self.resources.insert_len = 0 self.resources.next_points = None self.resources.motion_task = None self.resources.plan_task = None self.resources.mode_commands_list = deque([]) self.resources.target_points = None self.resources.is_first_move = True self.resources.is_jump = False self.resources.is_skip = False self.resources.is_insert = False self.resources.current_queue_num = -2 self.resources.is_acupoint = False #重置穴位展示标志 self.resources.robot.arm_state.massage_wrench = np.zeros(6) self.resources.temperature = self.resources.temperature_default self.resources.gear = self.resources.gear_default self.resources.shake = self.resources.shake_default self.resources.press = self.resources.press_default self.resources.frequency = self.resources.frequency_default self.resources.speed = self.resources.speed_default self.resources.direction = self.resources.direction_default self.resources.robot.arm_state.desired_high = self.resources.desired_high_default self.resources.robot.thermotherapy = None self.resources.robot.shockwave = None self.resources.robot.stone = None self.resources.robot.heat = None del self.resources.robot.ion self.resources.robot.ion = None self.resources.choose_task = None if self.resources.update_massageHead_temper_thread is not None: self.resources.update_massageHead_temper_thread.join() self.resources.update_massageHead_temper_thread = None self.resources.temper_head = None self.resources.is_pause = False self.resources.send_instruction(instruction, "ui_update_massage_status") self.resources.send_instruction(instruction="emergency_stop", target="language") # 这里可以处理一些清理工作 self.resources.logger.log_error("Emergency exit triggered, shutting down...") self.resources.robot.stop_thread() self.resources.robot.stop() # self.resources.robot.arm.power_off() self.resources.stop_event.clear() # 或者使用 sys.exit() 直接退出程序 # sys.exit(1) return "emergency_exit" async def handler(websocket, path, resources: SharedResources): async for message in websocket: resources.logger.log_info("received: " + message) resources.logger.log_info("now_state: " + resources.now_state) if ( message == "stop" and resources.now_state != "STOP" and resources.now_state != "IDLE" ): resources.stop_event.set() resources.robot.user_interrupt() if ( message == "pause" and resources.now_state != "STOP" and resources.now_state != "PAUSE" and resources.now_state != "IDLE" ): if resources.now_state == "SMART_DEAL" or resources.now_state == "MANUAL_DEAL": resources.send_instruction(instruction="no_pause", target="language") resources.send_instruction(instruction={"message": "我现在在拍照呢
暂时不可以暂停哦"}, target="ui_on_message") else: if resources.is_finish == False: resources.pause_event.set() resources.robot.user_pause() elif resources.is_finish == True: resources.send_instruction(instruction={"message": "按摩已经结束啦
马上就要停止了哦"}, target="ui_on_message") elif ( message == "skip_queue" and resources.now_state != "STOP" and resources.now_state != "PAUSE" and resources.now_state != "IDLE" and resources.now_state != "SMART_DEAL" and resources.now_state != "MANUAL_DEAL" and not resources.skip_event.set() and resources.robot.is_execute == True ): if resources.is_finish == False and resources.motion_task is not None: resources.send_instruction(instruction="skip_queue", target="language") print("skip_queue,待测试") resources.is_skip = True #跳过标志位置为True resources.skip_event.set() resources.robot.user_skip() elif "begin" in message and (resources.now_state == "IDLE" or resources.now_state == "PAUSE"): resources.send_instruction("check_license",target="check_license") if resources.license_check == True: resources.stop_event.clear() resources.robot.interrupt_event.clear() resources.begin_event.set() parts = message.split(":") print(f"begin command: {parts}") resources.robot.current_head = parts[1] + "_head" resources.choose_task = parts[1] resources.body_part = parts[3] resources.mode_real = int(parts[4]) if resources.is_pause == False: resources.task_time = int(parts[2]) resources.show_picture_type = 1 # 检查是否提供 use_mode，若没有则默认 "smart" resources.use_mode = parts[5] if len(parts) > 5 else "smart" print("change_resources.begin_event.:",resources.begin_event.is_set()) resources.logger.log_blue(( f"message: {message} , choose_task: {resources.choose_task} , " f"task_time: {resources.task_time}, body_part: {resources.body_part}, " f"mode_real: {resources.mode_real}, use_mode: {resources.use_mode}" f"loops: {resources.loops}" )) elif "begin" in message and resources.now_state == "MANUAL_DEAL" and resources.manual_start == False: resources.send_instruction("check_license",target="check_license") if resources.license_check == True: resources.stop_event.clear() resources.robot.interrupt_event.clear() parts = message.split(":") print(f"begin command: {parts}") resources.robot.current_head = parts[1] + "_head" resources.choose_task = parts[1] if resources.is_pause == False: resources.task_time = int(parts[2]) resources.body_part = parts[3] resources.mode_real = int(parts[4]) resources.use_mode = parts[5] resources.get_picture = True resources.selected_plan = None resources.show_picture_type = 1 resources.logger.log_blue(( f"message: {message} , choose_task: {resources.choose_task} , " f"task_time: {resources.task_time}, body_part: {resources.body_part}, " f"mode_real: {resources.mode_real}, use_mode: {resources.use_mode}" )) elif ( "adjust" in message # and resources.now_state != "STOP" # and resources.now_state != "IDLE" ): resources.adjust_event.set() adjust_type = message.split(":")[1] adjust_command = message.split(":")[2] adjust_degree = message.split(":")[3] resources.logger.log_info("adjust_type: " + adjust_type) if adjust_type == "pose": position,quat_rot = resources.robot.arm.get_arm_position() resources.position_increment = position_adjust( adjust_command, adjust_degree, quat_rot ) resources.logger.log_info( "processing adjust with position_increment: " + str(resources.position_increment) ) resources.robot.user_adjust( pose_increment=resources.position_increment ) # 修改当前点的位置，并且在任务重处理后续点的位置 elif adjust_type == "force": # resources.logger.log_info( # f"if adjust to force: {resources.robot.is_execute}" # ) # if resources.is_execute: resources.robot.arm_state.massage_wrench = force_adjust( adjust_command, adjust_degree, resources.robot.arm_state.massage_wrench, resources.force_limit, ) resources.logger.log_info( f"processing adjust to force: {resources.robot.arm_state.massage_wrench}" ) resources.send_instruction( {"force": str(-resources.robot.arm_state.massage_wrench[2])}, "ui_update_massage_status" ) resources.robot.user_adjust( force=resources.robot.arm_state.massage_wrench ) # 直接修改力度大小，不在任务中处理 elif adjust_type == "temperature": resources.temperature = temperature_adjust(adjust_command,adjust_degree,resources.temperature,resources.temperature_limit) resources.logger.log_info( f"processing adjust to force: {resources.temperature}" ) resources.send_instruction( {"temperature": str(resources.temperature)}, "ui_update_massage_status" ) resources.robot.user_adjust( temperature=resources.temperature ) if adjust_type == "speed": resources.speed = stone_speed_adjust(adjust_command,adjust_degree,resources.speed,resources.speed_limit) resources.logger.log_info( f"processing adjust to stone speed: {resources.speed}" ) resources.send_instruction( {"speed": str(resources.speed)}, "ui_update_massage_status" ) resources.robot.user_adjust( speed=resources.speed ) if adjust_type == "direction": if resources.direction == 1: resources.direction = 0 else: resources.direction = 1 resources.logger.log_info( f"processing adjust to stone dircetion: {resources.direction}" ) resources.send_instruction( {"direction": str(resources.direction)}, "ui_update_massage_status" ) resources.robot.user_adjust( direction=resources.direction ) elif adjust_type == "gear": resources.gear = gear_adjust(adjust_command,adjust_degree,resources.gear,resources.gear_limit) resources.logger.log_info( f"processing adjust to gear: {resources.gear}" ) resources.send_instruction( {"gear": str(resources.gear)}, "ui_update_massage_status" ) resources.robot.user_adjust( gear=resources.gear ) elif adjust_type == "shake": resources.shake = shake_adjust(adjust_command,adjust_degree,resources.shake,resources.shake_limit) resources.logger.log_info( f"processing adjust to shake: {resources.shake}" ) resources.send_instruction( {"shake": str(resources.shake)}, "ui_update_massage_status" ) resources.robot.user_adjust( shake=resources.shake ) elif adjust_type == "press": resources.press = press_adjust(adjust_command,adjust_degree,resources.press,resources.press_limit) resources.logger.log_info( f"processing adjust to press: {resources.press}" ) resources.send_instruction( {"press": str(resources.press)}, "ui_update_massage_status" ) resources.robot.user_adjust( press=resources.press ) elif adjust_type == "frequency": resources.frequency = frequency_adjust(adjust_command,adjust_degree,resources.frequency,resources.frequency_limit) resources.logger.log_info( f"processing adjust to frequency: {resources.frequency}" ) resources.send_instruction( {"frequency": str(resources.frequency)}, "ui_update_massage_status" ) resources.robot.user_adjust( frequency=resources.frequency ) elif adjust_type == "high": resources.robot.arm_state.desired_high = high_adjust(adjust_command,resources.robot.arm_state.desired_high,resources.high_limit) resources.logger.log_info( f"processing adjust to high: {resources.robot.arm_state.desired_high}" ) resources.send_instruction( {"high": str(round(resources.robot.arm_state.desired_high * 100.0))}, "ui_update_massage_status" ) elif ( message == "insert_queue" and resources.now_state != "STOP" and resources.now_state != "PAUSE" and resources.now_state != "IDLE" and resources.now_state != "SMART_DEAL" and resources.now_state != "MANUAL_DEAL" and resources.is_insert != True and resources.robot.is_execute == True ): if resources.is_finish == False and resources.motion_task is not None: resources.send_instruction(instruction="massage_time_longer", target="language") resources.is_insert = True #插入队列标志位置为True resources.insert_queue_thread = threading.Thread(target=resources.process_insert_queue) resources.insert_queue_thread.start() if "get_status" in message or "adjust" in message: instruction = {} instruction["massage_service_started"] = True instruction["is_massaging"] = ( True if (resources.now_state != "IDLE" and resources.now_state != "PAUSE") else False ) instruction["manual_stage"] = resources.manual_stage instruction["massage_head"] = ( resources.robot.current_head if resources.now_state != "IDLE" else False ) if resources.now_state != "IDLE": instruction["progress"] = str(resources.progress) resources.logger.log_error( "processing get_status with progress: " + str(resources.progress) ) instruction["force"] = str(-resources.robot.arm_state.massage_wrench[2]) instruction["current_head"] = resources.robot.current_head instruction["temperature"] = resources.temperature instruction["temper_head"] = None instruction["speed"] = resources.speed instruction["direction"] = resources.direction instruction["gear"] = resources.gear instruction["shake"] = resources.shake instruction["press"] = resources.press instruction["high"] = round(resources.robot.arm_state.desired_high * 100) instruction["frequency"] = resources.frequency instruction["task_time"] = str(resources.task_time) instruction["current_time"] = str(resources.current_time) instruction["loops"] = str(resources.loops) instruction["body_part"] = resources.body_part instruction["mode_real"] = resources.mode_real instruction["massage_state"] = resources.now_state instruction["is_acupoint"] = resources.is_acupoint instruction["use_mode"] = resources.use_mode instruction["is_pause"] = resources.is_pause instruction["start_pos"] = resources.motion_task[9] if resources.motion_task is not None else None instruction["end_pos"] = resources.motion_task[10] if resources.motion_task is not None else None instruction["massage_path"] = resources.motion_task[5] if resources.motion_task is not None else None else: instruction["progress"] = "0" instruction["force"] = "0" instruction["temperature"] = resources.temperature instruction["temper_head"] = None instruction["speed"] = resources.speed instruction["direction"] = resources.direction instruction["gear"] = resources.gear instruction["shake"] = resources.shake instruction["press"] = resources.press instruction["high"] = "" instruction["frequency"] = resources.frequency instruction["current_head"] = "" instruction["task_time"] = 0 instruction["current_time"] = 0 instruction["loops"] = 0 instruction["body_part"] = "" instruction["mode_real"] = 1 instruction["massage_state"] = resources.now_state instruction["is_acupoint"] = False instruction["use_mode"] = "smart" instruction["is_pause"] = resources.is_pause instruction["start_pos"] = None instruction["end_pos"] = None instruction["massage_path"] = "" resources.logger.log_yellow(instruction) resources.send_instruction(f"massage_status:{str(instruction)}", "language") resources.send_instruction(instruction, "ui_update_massage_status") elif "get_picture" in message and resources.now_state == "MANUAL_DEAL": resources.get_picture = True resources.show_picture_type = 2 resources.logger.log_info("processing get_picture") elif "cal_acu" in message and resources.now_state == "MANUAL_DEAL": try: # 检查 cal_acu 是否为 None if message.split(":")[1] == "None": resources.logger.log_info("Entering automatic recognition mode.") # 在此处添加进入自动识别模式的逻辑 resources.cal_acu = True resources.show_picture_type = 3 else: # 解析点的坐标 points_data = message.split(":")[1:] # 检查是否为新格式数据（包含额外参数） if len(points_data) < 10 and len(points_data) != 16: # 检查是否包含 10 个数值（5 个点，每点 x 和 y） resources.logger.log_error(f"Invalid points data format: {points_data}") else: # 将点解析为 [[x1, y1], [x2, y2], ...] 的格式 if resources.body_part == "back" or resources.body_part == "shoulder" or resources.body_part == "back_shoulder" or resources.body_part == "waist": points = [[int(points_data[i]), int(points_data[i + 1])] for i in range(0, len(points_data), 2)] resources.acupoint_key_data = {"shapes":[{"label": "back","points": [points[1],points[2],points[3],points[4]],"shape_type": "polygon"}, {"label": "back_center","points": [points[0]],"shape_type": "point"}]} elif resources.body_part == "belly": points = [[int(points_data[i]), int(points_data[i + 1])] for i in range(0, len(points_data), 2)] resources.acupoint_key_data = {"shapes":[{"label": "fubu","points": [points[1],points[2],points[3],points[4]],"shape_type": "polygon"}, {"label": "duqiyan","points": [points[0]],"shape_type": "point"}]} elif resources.body_part == "leg": # 提取前12个值作为点坐标（6个点） points = [[int(points_data[i]), int(points_data[i + 1])] for i in range(0, min(12, len(points_data)), 2)] # 提取可能的额外参数（如曲率等） extra_params = [round(float(val)*0.5) for val in points_data[12:]] if len(points_data) > 12 else [] # 为leg身体部位创建新的JSON结构 resources.acupoint_key_data = { "shapes": [ { "label": "C1", "points": [ [points[0][0], points[0][1]], [points[1][0], points[1][1]], [points[2][0], points[2][1]] ], "curvatures": { "thigh": extra_params[0], "calf": -extra_params[1] }, "shape_type": "linestrip" }, { "label": "C2", "points": [ [points[3][0], points[3][1]], [points[4][0], points[4][1]], [points[5][0], points[5][1]] ], "curvatures": { "thigh": extra_params[2], "calf": -extra_params[3] }, "shape_type": "linestrip" } ] } resources.logger.log_info(f"processing cal_acu with data: {resources.acupoint_key_data}") # 设置标志 resources.cal_acu = True resources.show_picture_type = 3 except Exception as e: resources.logger.log_error(f"processing cal_acu with error: {e}") elif "manual_start" in message and resources.now_state == "MANUAL_DEAL": resources.get_picture = False resources.cal_acu = False resources.manual_start = True resources.show_picture_type = 4 parts = message.split(":") if resources.is_pause == False: resources.task_time = int(parts[1]) resources.loops = int(parts[2]) resources.logger.log_blue("taske_time:",parts,resources.task_time) resources.logger.log_blue("loops:",parts,resources.loops) resources.logger.log_info("manual_start") elif "select_plan" in message and resources.now_state == "MANUAL_DEAL" and resources.manual_stage == 1: parts = message.split(":") if len(parts) == 2: resources.selected_plan = parts[1] resources.logger.log_info(f"processing select_plan with plan: {resources.selected_plan}") else: resources.logger.log_info("invalid command") await websocket.send(f"success received") def position_adjust(command, degree, quat_rot): """位置调整解算,返回位置增量""" if "left" in command: if degree == "high": position_increment = [0.02, 0, 0] elif degree == "low": position_increment = [0.01, 0, 0] else: position_increment = [0.015, 0, 0] elif "right" in command: if degree == "high": position_increment = [-0.02, 0, 0] elif degree == "low": position_increment = [-0.01, 0, 0] else: position_increment = [-0.015, 0, 0] elif "up" in command: if degree == "high": position_increment = [0, 0.05, 0] elif degree == "low": position_increment = [0, 0.025, 0] else: position_increment = [0, 0.04, 0] elif "down" in command: if degree == "high": position_increment = [0, -0.05, 0] elif degree == "low": position_increment = [0, -0.025, 0] else: position_increment = [0, -0.04, 0] rotation = R.from_quat(quat_rot) position_increment = rotation.apply(position_increment) return np.array(position_increment) def force_adjust(command, degree, massage_wrench, limit): """力度调整解算,返回力度""" print("force_limit:",limit) force_increment = 0 if "decrease" in command: if degree == "high": force_increment += 8 elif degree == "low": force_increment += 2 else: force_increment += 5 elif "increase" in command: if degree == "high": force_increment -= 8 elif degree == "low": force_increment -= 2 else: force_increment -= 5 massage_wrench[2] += force_increment massage_wrench[2] = np.clip(massage_wrench[2], limit[0], limit[1]) return np.array(massage_wrench) def temperature_adjust(command, degree, temperature, limit): temperature_increment = 0 if "decrease" in command: if degree == "high": temperature_increment = -2 elif degree == "low": temperature_increment = -1 else: temperature_increment = -1 elif "increase" in command: if degree == "high": temperature_increment = +2 elif degree == "low": temperature_increment = +1 else: temperature_increment = +1 new_temperature = temperature + temperature_increment return max(limit[0], min(new_temperature, limit[1])) def stone_speed_adjust(command, degree, speed, limit): speed_increment = 0 if "decrease" in command: if degree == "high": speed_increment = -2 elif degree == "low": speed_increment = -1 else: speed_increment = -1 elif "increase" in command: if degree == "high": speed_increment = +2 elif degree == "low": speed_increment = +1 else: speed_increment = +1 new_speed = speed + speed_increment return max(limit[0], min(new_speed, limit[1])) def gear_adjust(command, degree, gear, limit): gear_increment = 0 if "decrease" in command: if degree == "high": gear_increment = -2 elif degree == "low": gear_increment = -1 else: gear_increment = -1 elif "increase" in command: if degree == "high": gear_increment = +2 elif degree == "low": gear_increment = +1 else: gear_increment = +1 new_gear = gear + gear_increment return max(limit[0], min(new_gear, limit[1])) def shake_adjust(command, degree, shake, limit): shake_increment = 0 if "decrease" in command: if degree == "high": shake_increment = -2 elif degree == "low": shake_increment = -1 else: shake_increment = -1 elif "increase" in command: if degree == "high": shake_increment = +2 elif degree == "low": shake_increment = +1 else: shake_increment = +1 new_shake = shake + shake_increment return max(limit[0], min(new_shake, limit[1])) def press_adjust(command, degree, press, limit): press_increment = 0 if "decrease" in command: if degree == "high": press_increment = -3 elif degree == "low": press_increment = -1 else: press_increment = -2 elif "increase" in command: if degree == "high": press_increment = +3 elif degree == "low": press_increment = +1 else: press_increment = +2 new_press = press + press_increment return max(limit[0], min(new_press, limit[1])) def frequency_adjust(command, degree, frequency, limit): frequency_increment = 0 if "decrease" in command: if degree == "high": frequency_increment = -3 elif degree == "low": frequency_increment = -1 else: frequency_increment = -2 elif "increase" in command: if degree == "high": frequency_increment = +3 elif degree == "low": frequency_increment = +1 else: frequency_increment = +2 new_frequency = frequency + frequency_increment return max(limit[0], min(new_frequency, limit[1])) def high_adjust(command, high, limit): high_increment = 0 if "decrease" in command: high_increment = -0.01 elif "increase" in command: high_increment = +0.01 print("high command:",high_increment) print("limit",limit) new_high = high + high_increment print("new_high",new_high) return max(limit[0], min(new_high, limit[1])) def start_server(resources: SharedResources): async def handler_with_kwargs(websocket, path): await handler(websocket, path, resources) async def server(): async with websockets.serve(handler_with_kwargs, "localhost", 8765): resources.logger.log_info("WebSocket server started on ws://localhost:8765") await asyncio.Future() loop = asyncio.new_event_loop() asyncio.set_event_loop(loop) loop.run_until_complete(server()) def start_client(resources): asyncio.set_event_loop(resources.loop) resources.loop.run_forever() def create_state_change_callback(resources: SharedResources): def state_change_callback(userdata, active_states): resources.logger.log_info("state changed to: " + active_states[0]) resources.now_state = active_states[0] return state_change_callback def start_sm(resources): sm = smach.StateMachine(outcomes=["emergency_exit"]) with sm: smach.StateMachine.add( "IDLE", Idle(resources), transitions={ "to_smart": "SMART_DEAL", "to_manual": "MANUAL_DEAL", }, ) smach.StateMachine.add( "SMART_DEAL", SMART_DEAL(resources), transitions={ "to_thermotherapy": "THERMOTHERAPY_TASK", "to_shockwave": "SHOCKWAVE_TASK", "to_ball": "BALL_TASK", "to_finger": "FINGER_TASK", "to_roller": "ROLLER_TASK", "to_spheres": "SPHERES_TASK", "to_stone": "STONE_TASK", "to_heat": "HEAT_TASK", "to_ion": "ION_TASK", "pause": "PAUSE", "stop": "STOP", "emergency": "error_exit", }, ) smach.StateMachine.add( "MANUAL_DEAL", MANUAL_DEAL(resources), transitions={ "to_thermotherapy": "THERMOTHERAPY_TASK", "to_shockwave": "SHOCKWAVE_TASK", "to_ball": "BALL_TASK", "to_finger": "FINGER_TASK", "to_roller": "ROLLER_TASK", "to_spheres": "SPHERES_TASK", "to_stone": "STONE_TASK", "to_heat": "HEAT_TASK", "to_ion": "ION_TASK", "pause": "PAUSE", "stop": "STOP", "emergency": "error_exit", }, ) smach.StateMachine.add( "THERMOTHERAPY_TASK", Thermotherapy_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "SHOCKWAVE_TASK", Shockwave_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "BALL_TASK", Ball_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "STONE_TASK", Stone_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "HEAT_TASK", Heat_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "FINGER_TASK", Finger_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "ROLLER_TASK", Roller_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "SPHERES_TASK", Spheres_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "ION_TASK", Ion_Task(resources), transitions={"pause": "PAUSE","stop": "STOP", "emergency": "error_exit"}, ) smach.StateMachine.add( "PAUSE", Pause(resources), transitions={ "to_smart": "SMART_DEAL", "to_manual": "MANUAL_DEAL", "stop": "STOP", "emergency": "error_exit", }, ) smach.StateMachine.add( "STOP", Stop(resources), transitions={"to_idle": "IDLE", "emergency": "error_exit"}, ) smach.StateMachine.add( "error_exit", EmergencyExitState(resources), # 定义一个退出的状态 transitions={"emergency_exit": "emergency_exit"} # 定义为终止状态 ) sis = smach_ros.IntrospectionServer("server_name", sm, "/SM_ROOT") sis.start() sm.register_transition_cb(create_state_change_callback(resources)) outcome = sm.execute() # 根据状态机的执行结果处理 if outcome == "emergency_exit": print("State machine terminated with emergency exit.") sis.stop() def main(): exit_event = threading.Event() exit_event.clear() def signal_handler(signum, frame): resources.robot.stop() instruction = { "is_massaging": False, "is_pause": False, "massage_service_started": False, "massage_head": None, "progress": "0", "force": "0", "temperature": "", "temper_head": "", "speed": "", "direction": "", "gear": "", "shake": "", "press": "", "frequency": "", "current_head": "", "task_time": 0, "loops": 0, "current_time": 0, "body_part": "", "massage_state": "", "mode_real": 1, "use_mode": "smart", "manual_stage": 0 } resources.send_instruction(f"massage_status:{str(instruction)}", "language") exit_event.set() args = parse_controller_args() resources = SharedResources(args) signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) # 捕获 systemctl stop rospy.init_node("state_machine") print("inited ros node") resources.logger.log_blue('start_sm') sm_thread = threading.Thread(target=start_sm, args=(resources,), daemon=True) sm_thread.start() resources.logger.log_blue('start_client') threading.Thread(target=start_client, args=(resources,), daemon=True).start() resources.logger.log_blue('start_server') threading.Thread(target=start_server, args=(resources,), daemon=True).start() instruction = { "is_massaging": False, "is_pause": False, "massage_service_started": True, "massage_head":resources.robot.current_head, "progress":str(resources.progress), "force": str(-resources.robot.arm_state.massage_wrench[2]), "temperature": resources.temperature, "temper_head": None, "speed": resources.speed, "direction": resources.direction, "gear": resources.gear, "shake": resources.shake, "press": resources.press, "frequency": resources.frequency, "task_time": str(resources.task_time), "current_time": str(resources.current_time), "loops": str(resources.loops), "body_part": resources.body_part, "massage_state": resources.now_state, "mode_real": resources.mode_real, "use_mode": resources.use_mode } resources.send_instruction(instruction, "ui_update_massage_status") resources.send_instruction(f"massage_status:{str(instruction)}", "language") resources.send_instruction("connect_successfully", "language") while sm_thread.is_alive() and not exit_event.is_set(): time.sleep(0.1) sys.exit(0) if __name__ == "__main__": main()