3860 lines
181 KiB
Python
Executable File
3860 lines
181 KiB
Python
Executable File
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.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": "检测到输入手法异常<br>并尝试重新设置"}, 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": "请检查是否存在不合理的参数设置或过短的按摩长度<br>并尝试重新设置"}, 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": "您躺的位置有点不合适呢<br>请尝试往上躺一点呢"}, target="ui_on_message")
|
||
else:
|
||
self.send_instruction(instruction={"message": "您躺的位置有点不合适呢<br>请尝试往下躺一点呢"}, 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')
|
||
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
|
||
])
|
||
if self.mode_commands_list is None:
|
||
self.send_instruction(instruction={"message": "没有检测到穴位点<br>请重新拍照"}, 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)
|
||
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"获取最新的位置:{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": "我现在在拍照呢<br>暂时不可以暂停哦"}, 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": "未知的身体部位<br>请重新选择"}, 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": "未知的身体部位<br>请重新选择"}, 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": "没有检测到穴位点<br>请重新拍照"}, 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": "没有检测到穴位点<br>请重新拍照"}, 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": "即将开始按摩<br>暂时不可以暂停哦"}, 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)
|
||
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": "即将开始按摩<br>暂时不可以暂停哦"}, 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": "按摩头开启失败<br>请检查按摩头及连接状态"}, 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": "按摩头开启失败<br>请检查按摩头及连接状态"}, 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": "目标位置深度信息异常<br>请检测设备情况"}, 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": "您躺的位置有点不合适呢<br>请尝试往上躺一点呢"}, target="ui_on_message")
|
||
else:
|
||
self.resources.send_instruction(instruction={"message": "您躺的位置有点不合适呢<br>请尝试往下躺一点呢"}, 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(arm_ip=self.resources.robot.arm_config['arm_ip']) # 初始化实例化力传感器
|
||
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"
|
||
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": "检测到烫伤的风险<br>机器人停止按摩"}, 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()
|
||
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:] = [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}")
|
||
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)
|
||
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)
|
||
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()
|
||
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:] = [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)
|
||
pose = copy.deepcopy(self.resources.robot.arm.init_pos)
|
||
time.sleep(0.1)
|
||
self.resources.robot.arm.move_joint(pose)
|
||
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()
|
||
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": "我现在在拍照呢<br>暂时不可以暂停哦"}, 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": "按摩已经结束啦<br>马上就要停止了哦"}, 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()
|