massage_nova5初步

Massage/MassageControl/algorithms/arm_state.py

@@ -14,6 +14,10 @@ class ArmState:
         self.last_arm_orientation = np.array([0.0,0.0,0.0,1.0]) # [qx, qy, qz, qw]
         self.last_external_wrench_base = np.zeros(6,dtype=np.float64)
         self.last_external_wrench_tcp = np.zeros(6,dtype=np.float64)
+
+        # 传感器数据
+        self.positionerSensorData = 0.0
+        self.desired_high = 0.06
         
         # 目标状态
         self.desired_position = np.zeros(3,dtype=np.float64)

Massage/MassageControl/hardware/dobot_nova5.py

@@ -5,6 +5,7 @@ except:
 import threading
 from time import sleep,time
 import re
+from tools.log import CustomLogger
 import copy
 import numpy as np
 from scipy.spatial.transform import Rotation as R
@@ -35,8 +36,8 @@ import math
 '''
 
 class dobot_nova5:
-    def __init__(self,ip):
-        self.ip = ip
+    def __init__(self,arm_ip = '192.168.5.1'):
+        self.ip = arm_ip ## 机械臂IP地址
         self.dashboardPort = 29999 ## 一发一收-设置-控制及运动端口号
         self.feedFourPort = 30004 ## 第四实时反馈(8ms)反馈数据端口号
         self.dashboard = None ## 初始化控制及运动端口实例
@@ -60,6 +61,8 @@ class dobot_nova5:
                 self.ToolValue = -1
                 '''
         self.feedbackData = feedbackItem() # 自定义反馈数据结构体
+        self.logger = CustomLogger('Dobot_nova5',True)
+
 
     def parseResultId(self,valueRecv):
         # 解析返回值
@@ -170,12 +173,14 @@ class dobot_nova5:
                     self.dashboard.Stop()
                     self.dashboard.EmergencyStop(mode=1)
                     print("当前工作停止，机械臂处于急停状态")
+                    return -1
                 if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                     print("机械臂为暂停状态")
                     res = self.dashboard.Continue()
                     code = self.parseResultId(res)[0]
                     if code == 0:
                         print("机械臂继续已暂停的运动指令")
+                    return -1
                 if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                     print("该次运动完成")
                     break

Massage/MassageControl/tools/decorator_tools.py

@@ -0,0 +1,29 @@
+from functools import wraps
+
+def custom_decorator(before=None, after=None):
+    """
+    创建一个自定义装饰器，可以在函数调用之前和之后执行自定义操作。
+    
+    :param before: 在函数调用之前执行的操作，接受同样的参数。
+    :param after: 在函数调用之后执行的操作，接受同样的参数和返回值。
+    :return: 一个装饰器。
+    """
+    def decorator(func):
+        @wraps(func)
+        def wrapper(self, *args, **kwargs):
+            if before:
+                before(self, *args, **kwargs)
+            result = func(self, *args, **kwargs)
+            if after:
+                after(self, result, *args, **kwargs)
+            return result
+        return wrapper
+    return decorator
+
+def apply_decorators(cls):
+    for attr_name, attr_value in cls.__dict__.items():
+        if callable(attr_value) and hasattr(attr_value, '_decorator'):
+            decorated_func = attr_value._decorator(attr_value)
+            setattr(cls, attr_name, decorated_func)
+    return cls
+

Massage/MassageControl/tools/serial_tools.py

@@ -0,0 +1,94 @@
+import serial.tools.list_ports as list_ports
+import subprocess
+
+def find_serial_by_serial_number(serial_number):
+    ports = list_ports.comports()
+    for port in ports:
+        if port.serial_number == serial_number:
+            return port.device
+    return None
+
+def find_serial_by_location(location):
+    ports = list_ports.comports()
+    for port in ports:
+        if port.location == location:
+            return port.device
+    return None
+
+def list_usb_ports_with_details():
+    ports = list_ports.comports()
+    usb_ports = {}
+
+    for port in ports:
+        if "ttyUSB" in port.device:
+            usb_ports[port.device] = {
+                'serial_number': port.serial_number,
+                'vid': port.vid,
+                'pid': port.pid,
+                'location': port.location,
+                'description': port.description,
+                'manufacturer': port.manufacturer
+            }
+
+    return usb_ports
+
+# 维护 socat 进程的字典，用于跟踪每个虚拟串口的进程
+socat_processes = {}
+
+def start_virtual_serial(remote_ip, remote_port, device_name):
+    """
+    启动一个 socat 进程来创建虚拟串口。
+    :param remote_ip: 远程主机 IP 地址
+    :param remote_port: 远程主机端口
+    :param device_name: 虚拟串口的设备名称
+    """
+    # 构建 socat 命令
+    socat_command = f'socat PTY,raw,echo=0,link={device_name} TCP:{remote_ip}:{remote_port}'
+    # 启动 socat 进程
+    process = subprocess.Popen(socat_command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
+    # 将进程存储到字典中
+    socat_processes[device_name] = process
+    print(f"Started socat process for {device_name} on {remote_ip}:{remote_port}")
+
+def stop_virtual_serial(device_name):
+    """
+    停止一个 socat 进程。
+    :param device_name: 虚拟串口的设备名称
+    """
+    # 检查进程是否在字典中
+    if device_name in socat_processes:
+        # 获取进程对象
+        process = socat_processes[device_name]
+        # 使用 pkill 终止相应的 socat 进程
+        subprocess.call(['pkill', '-f', f'{device_name}'])
+        # 终止进程
+        process.terminate()
+        process.wait()
+        # 移除已终止的进程
+        del socat_processes[device_name]
+        print(f"Stopped socat process for {device_name}")
+    else:
+        print(f"No running socat process found for {device_name}")
+
+if __name__ == "__main__":
+    usb_ports_with_details = list_usb_ports_with_details()
+
+    if usb_ports_with_details:
+        print("USB Ports and their Details:")
+        for device, details in usb_ports_with_details.items():
+            print(f"Device: {device}, Serial Number: {details['serial_number']}, VID: {details['vid']}, PID: {details['pid']}, Location: {details['location']}, Description: {details['description']}, Manufacturer: {details['manufacturer']}")
+    else:
+        print("No USB ports found.")
+
+    # serial_number = "1234567890"  # Replace with your serial number
+    # port = find_serial_by_serial_number(serial_number)
+    # if port:
+    #     print(f"Serial number {serial_number} found on port {port}")
+    # else:
+    #     print(f"Serial number {serial_number} not found")
+
+    port = find_serial_by_location("1-8")
+    if port:
+        print(f"Port found: {port}")
+    else:
+        print("Port not found")