import copy import logging import random import threading import time import cv2 import numpy as np from PyQt5.QtWidgets import QMessageBox from PySide6.QtCore import Signal, QObject from PySide6.QtGui import QPixmap import Constant import Expection from CU.Catch import Catch, CatchStatus from CU.Detect import Detect, DetectStatus from Model.FeedModel import PositionModel from Model.Position import Real_Position, Detection_Position from enum import Enum, IntEnum from COM.COM_Robot import RobotClient, DetectType from Model.RobotModel import CMDInstructRequest, MoveType from Trace.handeye_calibration import getPosition from Trace.handeye_calibration import getxyz,getxyz1 from Util.util_math import get_distance from Util.util_time import CRisOrFall #from Vision.camera_coordinate_dete import Detection from Util.util_log import log from Model.RobotModel import Instruction from EMV.EMV import RelayController from CU.drop import DropPositionManager class ResetStatus(Enum): RNone = 0 RStart = 1 RRunging = 2 ROk =3 class FeedStatus(IntEnum): FNone = 0 FStart = 1 FCheck = 2 FMid = 3 FPhoto = 4 FTake = 5 FBroken1 = 6 FBroken2 =7 FShake = 8 FDropBag = 9 FFinished = 10 FReverse = 11 FStartReverse = 12 class LineType(Enum): Straight = 0 CureMid = 2 CureEnd = 3 WORLD = 4 class FeedMidStatus(Enum): FMid_Start = 1 FMid_Take = 2 FMid_Feed= 3 class FeedPosition: def __init__(self,status:FeedStatus,position:Real_Position): self.status = status self.position = position class FeedLine: def __init__(self, id, name, feed_positions:list): self.feed_positions = copy.deepcopy(feed_positions) self.feeding2end_pos_index = 0 self.origin2start_pos_index = 0 self.start2take_pos_index = 0 self.name = name self.id = id # --- 新增：用于存储从 ini 文件读取的多个投料点坐标 --- # 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...] self.drop_point_list = [] def get_current_feed_position(self,is_reverse): pos = self.feeding_to_end[ (self.feeding2end_pos_index+1)%len(self.feeding_to_end) if is_reverse else self.feeding2end_pos_index-1] return pos def get_current_take_position(self,is_reverse): pos = self.start_to_take[ (self.start2take_pos_index+1)%len(self.start_to_take) if is_reverse else self.start2take_pos_index-1] return pos def get_current_start_position(self,is_reverse): pos = self.origin_to_start[(self.origin2start_pos_index+1)%len(self.origin_to_start) if is_reverse else self.origin2start_pos_index-1] return pos def get_next_feed_position(self,reverse:bool=False): pos = self.feeding_to_end[self.feeding2end_pos_index] if reverse: self.feeding2end_pos_index -= 1 if self.feeding2end_pos_index < 0: self.feeding2end_pos_index = len(self.feeding_to_end) - 1 else: self.feeding2end_pos_index += 1 if self.feeding2end_pos_index >= len(self.feeding_to_end): self.feeding2end_pos_index = 0 return pos def get_next_start_position(self,reverse:bool=False): pos = self.origin_to_start[self.origin2start_pos_index] if reverse: self.origin2start_pos_index -= 1 if self.origin2start_pos_index < 0: self.origin2start_pos_index = len(self.origin_to_start) - 1 else: self.origin2start_pos_index += 1 if self.origin2start_pos_index >= len(self.origin_to_start): self.origin2start_pos_index = 0 return pos def get_next_take_position(self,reverse:bool=False): pos = self.start_to_take[self.start2take_pos_index] if reverse: self.start2take_pos_index -= 1 if self.start2take_pos_index < 0: self.start2take_pos_index = len(self.start_to_take) - 1 else: self.start2take_pos_index += 1 if self.start2take_pos_index >= len(self.start_to_take): self.start2take_pos_index = 0 return pos def get_take_position(self): for i in range(len(self.feeding_to_end)): if self.feeding_to_end[i].status == FeedStatus.FTake.value: return self.feeding_to_end[i] def set_take_position(self, position: Real_Position, dynamic_height=0): """ 设置 FTake 位置，并更新其前后动态点的位置。 :param position: 新的抓取位置 :param dynamic_height: 动态高度调整 (如果需要) """ for i in range(len(self.feeding_to_end)): if self.feeding_to_end[i].status == FeedStatus.FTake.value: # 计算 XYZ 坐标 # xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c) xyz = getxyz1(position.X, position.Y, position.Z, position.a, position.b, position.c) # 创建 before 和 after 位置 befor_take_position = Real_Position().init_position(xyz[0], xyz[1], xyz[2], position.U, position.V, position.W) after_take_position = Real_Position().init_position(xyz[0], xyz[1], xyz[2], position.U, position.V, position.W) # 安全检查索引 if i > 0: self.feeding_to_end[i - 1].set_position(befor_take_position) else: print("Warning: No position before FTake to update.") self.feeding_to_end[i].set_position(position) if i + 1 < len(self.feeding_to_end): self.feeding_to_end[i + 1].set_position(after_take_position) else: print("Warning: No position after FTake to update.") break # 抓料点暂时就一个 def set_drop_position(self, position: Real_Position): """ 设置 FDropBag 位置，只设置当前点，不处理前后点。 :param position: 新的丢包位置 """ for i in range(len(self.feeding_to_end)): if self.feeding_to_end[i].status == FeedStatus.FDropBag.value: # 直接设置当前点的位置 self.feeding_to_end[i].set_position(position) print( f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})") break # 假设只有一个丢包点 def get_position_list(self): index_start = -1 for i in range(len(self.feed_positions)): if self.feed_positions[i].status == FeedStatus.FCheck.value: index_start = i break for i in range(len(self.feed_positions)): if self.feed_positions[i].status == FeedStatus.FPhoto.value: index_take = i self.origin_to_start = self.feed_positions[: index_start+1] self.start_to_take = self.feed_positions[index_start:index_take+1] self.feeding_to_end = self.feed_positions[index_take:] for i in range(len(self.feeding_to_end)): #插入动态中间点 if self.feeding_to_end[i].status == FeedStatus.FTake.value: befor_position_model = PositionModel() befor_position_model.init_position(None) after_position_model = PositionModel() after_position_model.init_position(None) self.feeding_to_end.insert(i, befor_position_model) self.feeding_to_end.insert(i+2, after_position_model) break class FeedingConfig: def __init__(self, num: int, feedLine: FeedLine, photo_locs): self.num = num self.feedLine = feedLine self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs] def deal_photo_locs(self, photo_loc): position_photo = Real_Position() position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5]) return position_photo def get_line_info(self): pass class Feeding(QObject): need_origin_signal = Signal(str) take_no_photo_sigal = Signal() update_detect_image = Signal(np.ndarray) log_signal = Signal(int,str) def __init__(self, robotClient: RobotClient): super().__init__() self.feedConfig = None self.feedStatus = FeedStatus.FNone self.robotClient = robotClient # 添加 RelayController 实例 self.relay_controller = RelayController() self.sensor_thread = None self.detection_image = None self.init_detection_image() self.pause = False self.cRis_photo = CRisOrFall() self.cRis_shake = CRisOrFall() self.feed_Mid_Status = FeedMidStatus.FMid_Start self.is_reverse = False # 复位集合 self.run_reverse = False self.take_no_photo = False self.reset_status = ResetStatus.RNone self.reversed_positions = [] self.current_position = None self.index=1 self.pos_index = -1 self.pos_near_index = -1 self.catch = Catch(self.robotClient) self.detect = Detect() self.is_detected = True self.detect_thread = threading.Thread(target=self.run_detect) self.detect_thread.start() self.onekey = False self.debug_run_count = 0 # 初始化计数器 self.mid_take_count = 0 #传感器判断抓包参数 self.sensor2_ready = False # 传感器2是否检测到料包 self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机 self.sensor_thread = None self.relay_controller = RelayController() # 启动传感器2线程 self.relay_controller._running = True self.sensor2_thread = None # --- 新增: 用于码垛模式的投料点索引 --- self.current_drop_index = 0 self.drop_manager = DropPositionManager("CU/drop.ini") pass def close_feed(self): self.is_detected = False self.detect_thread.join() self.detect.detection.release() def init_detection_image(self): detection_image = cv2.imread(Constant.feed_sign_path) self.update_detect_image.emit(detection_image) def run_detect(self): while self.is_detected: self.detect.run() time.sleep(0.02) def run(self): self.catch.run() # 获取事件坐标 real_position = Real_Position() self.detect.position_index = 0 real_position.init_position(self.robotClient.status_model.world_0, self.robotClient.status_model.world_1, self.robotClient.status_model.world_2, self.robotClient.status_model.world_3, self.robotClient.status_model.world_4, self.robotClient.status_model.world_5) # real_position.init_position(0, # 0, # 0, # 0, # 0, # 0); # img_path = f'Image/{self.index}.png' # img = cv2.imread(img_path) # self.index += 1 # self.index = self.index % 4 # self.detection_image = img if self.feedConfig == None: self.feedStatus = FeedStatus.FNone if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position,is_action=True): self.feedStatus = FeedStatus.FNone self.is_reverse = False self.log_signal.emit(logging.INFO, Constant.str_feed_reverse) if self.feedStatus == FeedStatus.FNone or self.pause: return elif self.feedStatus == FeedStatus.FCheck: self.log_signal.emit(logging.INFO, Constant.str_feed_check) # 1, 检查是否是三列 # 2, 检查是否有人 # if self.safe_check_columns() and self.safe_check_person(): # pass # else: # if self.feedConfig.num != 0: # self.next_target() # if == 原点 继续判断 # else: # QMessageBox.information(None, "提示", Constant.str_feed_sae_error_msgbox) if self.is_reverse: self.feed_Mid_Status = FeedMidStatus.FMid_Start else: self.feed_Mid_Status = FeedMidStatus.FMid_Take if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position): self.next_position(self.is_reverse) elif self.feedStatus == FeedStatus.FStart: self.log_signal.emit(logging.INFO, Constant.str_feed_start) self.relay_controller.open(conveyor2=True)#开电机 #self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢 #self.sensor2_thread.start() if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse: # QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常 self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error) self.need_origin_signal.emit(Constant.str_feed_start_error) self.feedStatus = FeedStatus.FNone return if self.is_reverse: self.feedStatus = FeedStatus.FNone self.is_reverse = False return self.feedConfig.feedLine.get_position_list() self.detect.detect_status = DetectStatus.DNone self.feed_Mid_Status = FeedMidStatus.FMid_Start self.next_position(self.is_reverse) elif self.feedStatus == FeedStatus.FMid: feed_pos = self.get_current_position(self.is_reverse) if feed_pos.get_position().compare(real_position): self.log_signal.emit(logging.INFO, Constant.str_feed_mid) self.next_position(self.is_reverse) # 增加计数器逻辑 self.mid_take_count += 1 # 可选：在 Debug1 模式下输出日志 if Constant.Debug1: self.log_signal.emit( logging.INFO, f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次" ) #if self.feedStatus == FeedStatus.FTake: #self.catch.catch_status = CatchStatus.CTake elif self.feedStatus == FeedStatus.FPhoto: if self.feedConfig.num == 0: self.log_signal.emit(logging.INFO, Constant.str_feed_finish) self.is_reverse = True self.feed_Mid_Status = FeedMidStatus.FMid_Take self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2 self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2 self.next_position(self.is_reverse) self.log_signal.emit(logging.INFO, Constant.str_feed_photo) self.init_detection_image() return if not Constant.Debug: self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto) self.feed_Mid_Status = FeedMidStatus.FMid_Feed self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success) self.feedConfig.feedLine.set_take_position(self.detect.detect_position, 0) #self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置 '''real_position''' # 一直等待传感器2信号，永不超时 while True: sensors = self.relay_controller.get_all_device_status('sensors') sensor2_value = sensors.get(self.relay_controller.SENSOR2, False) if sensor2_value: self.log_signal.emit(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取") break # ✅ 条件满足，跳出循环，继续执行下面的代码 else: self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...") time.sleep(1) # 每秒检查一次 self.next_position() self.log_signal.emit(logging.INFO, Constant.str_sys_runing2) #self.feedStatus = FeedStatus.FTake elif self.feedStatus == FeedStatus.FTake: self.log_signal.emit(logging.INFO, Constant.str_feed_take) take_position = self.feedConfig.feedLine.get_take_position() if not take_position or not take_position.get_position(): self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail) return if not take_position.get_position().compare(real_position, is_action=True): self.log_signal.emit(logging.INFO, "🟡 机器人尚未到达抓料点位") return self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位") # 执行抓取动作 #self.catch.catch_status = CatchStatus.CTake #if self.catch.catch_status == CatchStatus.CNone: #self.catch.catch_status = CatchStatus.CTake #if self.catch.catch_status == CatchStatus.CTake: #self.log_signal.emit(logging.INFO, "正在执行抓料动作...") #self.catch.catch_status = CatchStatus.COk #if self.catch.catch_status == CatchStatus.COk: #self.log_signal.emit(logging.INFO, Constant.str_feed_take_success) #self.catch.catch_status = CatchStatus.CNone # 移动到下一个抓取点 # 更新丢包点: 如果需要根据放置情况调整下次抓取 next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1) print("next_drop_pos--------",next_drop_pos) if next_drop_pos: self.feedConfig.feedLine.set_drop_position(next_drop_pos) self.log_signal.emit(logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}") else: self.log_signal.emit(logging.ERROR, "获取放置点失败") return #self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点 self.relay_controller.open(clamp=True) self.next_position() #return #else: #self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail) elif self.feedStatus == FeedStatus.FBroken1: if self.get_current_position().get_position().compare(real_position): self.log_signal.emit(logging.INFO, Constant.str_feed_broken) self.next_position() elif self.feedStatus == FeedStatus.FBroken2: if self.get_current_position().get_position().compare(real_position): self.log_signal.emit(logging.INFO, Constant.str_feed_broken) self.next_position() elif self.feedStatus == FeedStatus.FShake: if self.get_current_position().get_position().compare(real_position,is_action=True): # TODO 震动方案 self.log_signal.emit(logging.INFO, Constant.str_feed_shake) if self.catch.catch_status == CatchStatus.CNone: self.catch.catch_status = CatchStatus.CShake return if self.catch.catch_status == CatchStatus.CShake: # if self.feedConfig.feedLine.feeding_to_end[ # self.feedConfig.feedLine.feeding2end_pos_index + 1].status != FeedStatus.FShake: # self.catch.catch_status = CatchStatus.COk # else: self.catch.shake_continue.SetReset() self.next_position() if self.feedStatus!=FeedStatus.FShake: self.catch.catch_status = CatchStatus.CNone return elif self.feedStatus == FeedStatus.FDropBag: #"""*** 处理投料点 (FDropBag) 的核心逻辑 ***""" # 1. 确认机械臂是否已精确到达当前目标投料点 # get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed) # 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取 # 由 feeding2end_pos_index 指向的点。 if self.get_current_position().get_position().compare(real_position, is_action=True): # 2. 记录日志：已到达投料点 #if not self.sensor2_ready: #self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2") #self.relay_controller.open(conveyor2=True) self.log_signal.emit(logging.INFO, Constant.str_feed_drop) # 3. 与 Catch 模块进行状态交互来驱动投料动作 # a. 初始状态 (CNone): 触发投料动作 if self.catch.catch_status == CatchStatus.CNone: # 将 Catch 状态设置为 CDrop，通知 Catch 模块开始执行物理投料动作 self.catch.catch_status = CatchStatus.CDrop # 立即返回，等待 Catch 模块处理 return # b. 投料进行中 (CDrop): 等待完成 if self.catch.catch_status == CatchStatus.CDrop: # 什么都不做，等待 Catch 模块完成动作并更新状态 return # c. 投料完成 (COk): 处理后续逻辑并移动到下一个点 if self.catch.catch_status == CatchStatus.COk: # 重置 Catch 状态，为下一次操作做准备 self.catch.catch_status = CatchStatus.CNone # (后续增加) 延时: 让物料稳定 # time.sleep(self.robotClient.time_delay_put) # (后续增加) 视觉确认: 拍照确认袋子已放置 # self.detection.get_position(...) # self.feedConfig.feedLine.set_take_position(...) # 4. 更新业务逻辑：减少剩余袋数 self.feedConfig.num = self.feedConfig.num - 1 self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') # 5. *** 关键步骤 ***: 移动到路径中的下一个点 # next_position() 会根据当前的 feed_Mid_Status (FMid_Feed) # 调用 next_Feed()。 self.next_position() def run_reset(self): real_position = Real_Position() real_position.init_position(self.robotClient.status_model.world_0, self.robotClient.status_model.world_1, self.robotClient.status_model.world_2, self.robotClient.status_model.world_3, self.robotClient.status_model.world_4, self.robotClient.status_model.world_5) if self.reset_status == ResetStatus.RNone: return if self.reset_status == ResetStatus.RStart: if self.feedConfig == None: return # for index in range(len(self.feedConfig.feedLine.positions)): # if self.feedConfig.feedLine.positions[index].status == 2: # start_index = index self.pos_index = -1 self.pos_near_index = -1 self.reversed_positions = [] for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): if pos_model.get_position().compare(real_position,is_action=True): self.pos_index = index break if self.pos_index == -1: self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail) min_distance = 99999999 for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): if get_distance(pos_model.get_position(), real_position) < min_distance: min_distance = get_distance(pos_model.get_position(), real_position) self.pos_near_index = index if self.pos_near_index != -1: self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1] else: return False else: self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index+1] self.reversed_positions = list(reversed(self.reversed_positions)) self.reverse_index = 0 self.send_emergency_sound() self.current_position = PositionModel() self.current_position.init_position(real_position) self.reset_status = ResetStatus.RRunging if self.reset_status == ResetStatus.RRunging: if not real_position.compare(self.current_position.get_position(),is_action=True): return pos_model = self.reversed_positions[self.reverse_index] if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点 pos_model = self.reversed_positions[self.reverse_index + 1] self.reverse_index = self.reverse_index+1 if pos_model.lineType == LineType.CureMid.value: pos_model1 = self.reversed_positions[self.reverse_index + 1] self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure, real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed) self.current_position = pos_model1 self.reverse_index = self.reverse_index + 2 else: self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed) self.current_position = pos_model self.reverse_index = self.reverse_index + 1 if self.reverse_index == len(self.reversed_positions): self.reset_status = ResetStatus.ROk if self.reset_status == ResetStatus.ROk: self.reset_status = ResetStatus.RNone self.send_emergency_stop() def return_original_position(self): self.run_reverse = True real_position = Real_Position() real_position.init_position(self.robotClient.status_model.world_0, self.robotClient.status_model.world_1, self.robotClient.status_model.world_2, self.robotClient.status_model.world_3, self.robotClient.status_model.world_4, self.robotClient.status_model.world_5) if self.feedConfig == None: return start_index = -1 # for index in range(len(self.feedConfig.feedLine.positions)): # if self.feedConfig.feedLine.positions[index].status == 2: # start_index = index pos_index = -1 pos_near_index = -1 reversed_positions = [] for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): if pos_model.get_position().compare(real_position): pos_index = index break if pos_index == -1: self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail) min_distance = 99999999 for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): if get_distance(pos_model.get_position(), real_position)