测试了密胺的整个流程、add(点位设置界面增加了 停止功能按钮)、fix bug(修复了点位名称写入数据库不一致的错误)

app/service/robot_service.py

@@ -0,0 +1,394 @@
+import threading
+from typing import List
+
+# # 法奥机器人
+from fairino import Robot
+
+# 夹具和吸盘控制
+from .catch import CatchTool
+
+# 震动函数
+from .vibrate import start_vibrate
+
+# 称重函数
+from .weight import start_weight
+
+# 倒料函数
+from .drop import start_drop, start_drop_rotate, start_drop_reset
+
+# 高周波
+from .high_machine import start_high
+
+# 冲压机
+from .press_machine import start_press
+
+import time
+
+
+class RobotService:
+    def __init__(self, robot_client: Robot.RPC):
+        self.robot_client = robot_client  # 机器臂实例
+
+        self.move_thread = None  # 移动任务线程对象
+        self.moving_running = False  # 移动线程运行状态标记
+
+        # 跟踪线程相关
+        # 跟踪线程的机器臂调用
+        self.robot_tracking_client = Robot.RPC(robot_client.ip_address)
+        self.point_sequence = []  # 点位序列元数据 （保存点位状态status和索引index）
+        self.current_target_index = -1  # 当前正在朝向（移动）的点位索引（初始为-1）
+        self.completed_indices = set()  # 已运动过的点位索引集合
+        self.tracking_lock = threading.Lock()
+
+        # 末端工具对象
+        self.catch_tool = CatchTool()
+
+    def start_moves(
+        self,
+        pos_list: List[list],
+        name_list: List[str],
+        tool_id: int = 1,
+        vel: float = 5.0,
+    ):
+        """启动移动任务（对外接口）"""
+        # 校验参数：坐标列表和名称列表长度必须一致
+        if len(pos_list) != len(name_list):
+            raise ValueError("pos_list与name_list长度不一致")
+
+        # 如果已有移动任务线程在运行，先停止
+        if self.move_thread and self.move_thread.is_alive():
+            self.stop_moves()
+
+        # 保存任务参数
+        self.pos_list = pos_list
+        self.name_list = name_list
+        self.tool_id = tool_id
+        self.vel = vel
+        self.moving_running = True
+
+        # 初始化点位序列
+        self.init_point_sequence()
+
+        # 启动移动点位跟踪
+        self.start_tracking()
+
+        # 启动移动任务线程, 执行移动任务
+        self.move_thread = threading.Thread(target=self._process_moves)
+        self.move_thread.daemon = True
+        self.move_thread.start()
+
+    def stop_moves(self):
+        """停止移动任务"""
+        self.moving_running = False
+
+        if self.stop_motion() == 0:
+            print("stop_moves: 机器臂停止移动成功...")
+
+        # 打印 停止时，正在朝哪个点移动
+        point_dict = self.get_current_target_point()
+        if point_dict:
+            print("停止前, 正在朝向移动的目标点位为：")
+            print(
+                f"所选中的所有点中的第{point_dict['index']+1} 个点",
+                "pos:",
+                point_dict["pos"],
+            )
+        else:
+            print("所有的点位已经移动完成, 无朝向移动的目标点!!!!")
+
+        # 停止跟踪
+        self.stop_tracking()
+
+    def stop_motion(self):
+        error = self.robot_client.send_message("/f/bIII4III102III4IIISTOPIII/b/f")
+        return error
+
+    def _process_moves(self):
+        """线程内执行的移动任务逻辑"""
+        for i in range(len(self.pos_list)):
+            if not self.moving_running:
+                break  # 若已触发停止，则退出循环
+
+            current_pos = self.pos_list[i]
+            current_name = self.name_list[i]
+            print(f"坐标名: {current_name}，坐标：{current_pos}")
+
+            if current_name == "normal":
+                # 中间的普通点加入平滑时间
+                error_code = self.robot_client.MoveCart(
+                    desc_pos=current_pos,
+                    tool=self.tool_id,
+                    user=0,
+                    vel=self.vel,
+                    blendT=200,  # 平滑, 200毫秒
+                )
+                # 加了平滑会出现一个问题，那就是该函数不会阻塞，就直接返回了
+            else:
+                # 执行机器臂移动
+                error_code = self.robot_client.MoveCart(
+                    desc_pos=current_pos, tool=self.tool_id, user=0, vel=self.vel
+                )
+
+            # 根据移动结果处理
+            # 必须要根据函数返回来决定
+            if self.moving_running and error_code == 0:
+                print(f" {current_name} 移动成功 或者 移动指令发送成功!!! ")
+                # 根据点位名称调用对应函数（核心判断逻辑）
+                # 功能点会调用相应的函数来进行相应的操作
+                # 注意: 功能点不能设置为 平滑点!!!!! 否则不能根据函数返回判断是否已经运动到该点位
+                self._call_action_by_name(current_name)  # 可以优化
+            else:
+                # 1、该点位移动失败....
+                # 2、机器臂停止运动，还会进入到这里....
+                print(
+                    f"移动到 {current_name} 失败，错误码：{error_code}, 运行状态: {self.moving_running}"
+                )
+                # 可选：是否继续执行下一个点？
+                self.moving_running = False
+
+        # 所有移动任务完成
+        self.moving_running = False
+        print("================所有点位任务已完成===============")
+
+    def _call_action_by_name(self, point_name: str):
+        """根据点位名称调用对应动作（核心判断逻辑）"""
+        # 1. 夹取相关点位
+        if point_name in ["抓取点", "夹取点"]:
+            self.catch_tool.gripper_grasp()
+
+        # 2. 放开相关点位
+        elif point_name in ["放开点"]:
+            self.catch_tool.gripper_release()
+
+        # 3. 称重相关点位
+        elif point_name in ["称重点"]:
+            start_weight()
+
+        # 4. 震频点相关点位
+        elif point_name in ["震频点", "震动点"]:
+            start_vibrate()
+
+        # 5. 倒料相关点位
+        elif point_name in ["倒料点"]:
+            start_drop(self.robot_client)
+
+        # start_high
+        elif point_name in ["高周波点"]:
+            start_high()
+
+        # start_press
+        elif point_name in ["冲压点", "冲压机点"]:
+            start_press()
+
+        elif point_name in ["冲压机等待点"]:
+            # 冲压机花费60秒，这里等待一段时间
+            print("等待冲压机冲压完成, 等待时间为40秒......")
+            # 因为高周波点等待了80秒，所以这里只需要等待40秒，一共120秒
+            # 这里会一起计算高周波等待点的80秒
+            time.sleep(40)
+            print("冲压已经完成......")
+
+        # start_drop_rotate
+        # 旋转180°倒料
+        elif point_name in ["倒料旋转点", "旋转点"]:
+            start_drop_rotate(self.robot_client)
+
+        # 旋转复原点
+        elif point_name in ["倒料复原点", "旋转复原点"]:
+            start_drop_reset(self.robot_client)
+
+        # 吸取点
+        # 吸盘吸取
+        elif point_name in ["吸取点"]:
+            self.catch_tool.suction_pick()
+
+        # 释放点
+        # 吸盘释放
+        elif point_name in ["释放点"]:
+            self.catch_tool.suction_release()
+
+        elif point_name in ["冲压机等待点2"]:
+            # 冲压机花费90秒，这里等待一段时间
+            # 这里是专门等待冲压机的....，不会一起计算高周波等待点的时间...
+            print("等待冲压机冲压完成, 等待时间为90秒......")
+            time.sleep(90)
+            print("冲压已经完成......")
+
+        # 4. 可扩展其他功能点位类型
+        # elif point_name in ["XXX点"]:
+        #     self._xxx_action()
+
+        # else:
+        #     print(f"点位 {point_name} 无对应动作，不执行额外操作")
+
+    def init_point_sequence(self):
+        """初始化点位序列，添加元数据"""
+        self.point_sequence.clear()  # 清空
+        for idx, point in enumerate(self.pos_list):
+            self.point_sequence.append(
+                {
+                    "index": idx,
+                    "pos": point,  # 坐标 [x,y,z,rx,ry,rz]
+                    "status": "pending",  # 状态：pending(未发送)/sent(已发送)/completed(已经过)/current(当前目标)
+                }
+            )
+        self.current_target_index = (
+            0 if self.point_sequence else -1
+        )  # 初始目标为第一个点
+
+    def get_robot_status(self):
+        """获取机器人实时状态：是否在运动 + 当前位置"""
+        try:
+            with self.tracking_lock:
+                # 1. 判断是否正在运动
+                is_moving = self.is_moving()
+
+                # 2. 获取当前TCP位置
+                # [x,y,z,rx,ry,rz]
+                result = self.robot_tracking_client.GetActualTCPPose()
+
+            # 解析返回值
+            if isinstance(result, tuple) and len(result) >= 2 and result[0] == 0:
+                # 成功：提取坐标列表（第二个元素）
+                current_pos = result[1]
+                # 校验坐标格式（6个数字）
+                if not (
+                    isinstance(current_pos, list)
+                    and len(current_pos) == 6
+                    and all(isinstance(p, (int, float)) for p in current_pos)
+                ):
+                    print(f"坐标格式错误：{current_pos}, 需为6个数字的列表")
+                    return None
+            else:
+                # 失败：打印错误码
+                error_code = result[0] if isinstance(result, tuple) else result
+                print(f"get_robot_status: 获取TCP位置失败, 错误码：{error_code}")
+                return None
+
+            return {
+                "is_moving": is_moving,
+                "current_pos": current_pos,
+                "timestamp": time.time(),
+            }
+        except Exception as e:
+            print(f"get_robot_status: 获取状态失败：{e}")
+            return None
+
+    def is_moving(self) -> bool:
+        """
+        判断机器臂是否正在运动
+
+        返回：
+            bool: True表示正在运动（robot_state=2），False表示停止/暂停/拖动/异常
+        """
+        # 1. 检查状态包是否已初始化
+        if self.robot_client.robot_state_pkg is None:
+            print("is_moving: 警告：机器人状态包未初始化，无法判断运动状态")
+            return False
+
+        try:
+            # 2. 从状态包中获取robot_state（取值1-4）
+            time.sleep(0.05)  # 等待状态刷新
+            robot_state = self.robot_client.robot_state_pkg.robot_state
+
+            # 3. 校验robot_state是否在合法范围内
+            if robot_state not in (1, 2, 3, 4):
+                print(
+                    f"is_moving: 警告: 无效的robot_state值 {robot_state}，按'停止'处理"
+                )
+                return False
+
+            # 4. 仅robot_state=2表示正在运动
+            return robot_state == 2
+
+        except Exception as e:
+            print(f"is_moving: 获取运动状态失败：{str(e)}")
+            return False
+
+    def update_point_status(self):
+        """更新点位状态：哪些已完成，当前移动的目标点位是哪个"""
+        status = self.get_robot_status()
+        if not status:
+            return
+
+        current_pos = status["current_pos"]
+        is_moving = status["is_moving"]
+        threshold = 6.0  # 判定“经过该点”的距离阈值（mm），根据精度调整
+
+        # 遍历所有点位，判断是否已经过
+        for point in self.point_sequence:
+            idx = point["index"]
+            # 跳过已标记为“已完成”的点
+            if point["status"] == "completed":
+                continue
+
+            # 计算当前位置与点位的三维距离
+            dx = current_pos[0] - point["pos"][0]
+            dy = current_pos[1] - point["pos"][1]
+            dz = current_pos[2] - point["pos"][2]
+            distance = (dx**2 + dy**2 + dz**2) ** 0.5
+
+            # 如果距离小于阈值，标记为“已完成”
+            if distance < threshold:
+                self.completed_indices.add(idx)
+                point["status"] = "completed"
+                max_completed = (
+                    max(self.completed_indices) if self.completed_indices else -1
+                )
+                self.current_target_index = max_completed + 1
+                if 0 <= self.current_target_index < len(self.point_sequence):
+                    self.point_sequence[self.current_target_index]["status"] = "current"
+
+        # 停止移动时判断当前目标点
+        if not is_moving and 0 <= self.current_target_index < len(self.point_sequence):
+            target_point = self.point_sequence[self.current_target_index]
+            if target_point["status"] != "completed":
+                dx = current_pos[0] - target_point["pos"][0]
+                dy = current_pos[1] - target_point["pos"][1]
+                dz = current_pos[2] - target_point["pos"][2]
+                distance = (dx**2 + dy**2 + dz**2) ** 0.5
+                if distance < threshold:
+                    self.completed_indices.add(self.current_target_index)
+                    target_point["status"] = "completed"
+                    if self.current_target_index + 1 < len(self.point_sequence):
+                        # 将下一个点设置为目前正在朝向移动的点
+                        self.current_target_index += 1
+                        self.point_sequence[self.current_target_index][
+                            "status"
+                        ] = "current"
+
+    def start_tracking(self):
+        """清空已经移动的点位的集合, 开启下一轮跟踪"""
+        self.completed_indices.clear()
+
+        """启动实时跟踪线程"""
+        self.tracking_running = True
+        self.tracking_thread = threading.Thread(target=self._tracking_loop, daemon=True)
+        self.tracking_thread.start()
+
+    def _tracking_loop(self):
+        """跟踪循环：持续更新点位状态"""
+        while self.tracking_running:
+            self.update_point_status()
+            time.sleep(0.2)  # 5Hz更新频率
+
+    def stop_tracking(self):
+        """停止跟踪线程"""
+        self.tracking_running = False
+        if self.tracking_thread.is_alive():
+            self.tracking_thread.join()
+
+    def get_completed_points(self):
+        """返回已运动过的点（索引和坐标）"""
+        return [
+            {"index": point["index"], "pos": point["pos"]}
+            for point in self.point_sequence
+            if point["status"] == "completed"
+        ]
+
+    def get_current_target_point(self):
+        """返回当前正在朝向移动的点（索引和坐标）"""
+        if 0 <= self.current_target_index < len(self.point_sequence):
+            point = self.point_sequence[self.current_target_index]
+            return {"index": point["index"], "pos": point["pos"]}
+        return None  # 无当前朝向移动的目标点位（已完成所有点位的移动）