import logging import time import cv2 import Constant from Model.Position import Real_Position, Detection_Position from enum import Enum, IntEnum from COM.COM_Robot import RobotClient from Model.RobotModel import CMDInstructRequest, MoveType from Vision.camera_coordinate_dete import Detection from Util.util_log import log class FeedStatus(IntEnum): FNone = 0 FStart = 1 FSafeP = 2 FPhoto = 3 FTake = 4 FSafeF = 5 FFeedP = 6 FBroken = 7 FFinished = 8 class FeedLine: def __init__(self,name,safe_position:Real_Position,photo_position:Real_Position,mid_position:Real_Position,broken_position:Real_Position,feed_position:Real_Position): self.safe_position = safe_position self.photo_position = photo_position self.feed_position = feed_position self.mid_position = mid_position self.broken_position = broken_position self.take_position = None self.name = name class FeedingConfig: def __init__(self, num:int, feedLine:FeedLine): self.num = num self.feedLine = feedLine def get_line_info(self): pass class Feeding(): def __init__(self,robotClient:RobotClient,detection:Detection): self.feedConfig = None self.feedStatus = FeedStatus.FNone self.robotClient = robotClient self.detection = detection self.detection_image = None self.init_detection_image() self.pause = False pass def init_detection_image(self): self.detection_image = cv2.imread(Constant.feed_sign_path) def run(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) # real_position.init_position(0, # 0, # 0, # 0, # 0, # 0); if self.feedConfig == None: self.feedStatus = FeedStatus.FNone elif self.feedConfig.num == 0: self.feedStatus = FeedStatus.FNone if self.feedStatus==FeedStatus.FNone or self.pause: return elif self.feedStatus==FeedStatus.FStart: log.log_message(logging.INFO,Constant.str_feed_start) if self.feedConfig.num != 0: self.feedStatus = FeedStatus.FSafeP self.sendTargPosition(self.feedConfig.feedLine.safe_position) # print(request_command) pass elif self.feedStatus==FeedStatus.FSafeP: log.log_message(logging.INFO,Constant.str_feed_safe) if self.feedConfig.feedLine.safe_position.compare(real_position): self.feedStatus = FeedStatus.FPhoto self.sendTargPosition(self.feedConfig.feedLine.photo_position) elif self.feedStatus == FeedStatus.FPhoto: log.log_message(logging.INFO, Constant.str_feed_photo) if self.feedConfig.feedLine.photo_position.compare(real_position): code, img, xyz, uvw = self.detection.get_position() #检测结果 log.log_message(logging.INFO, Constant.str_feed_takePhoto) self.detection_image = img if xyz!=None: log.log_message(logging.INFO, Constant.str_feed_takePhoto_success) #dp = Detection_Position().init_position(*xyz, *uvw) from Trace.handeye_calibration import R_matrix,getPosition rotation = R_matrix(self.robotClient.status_model.world_0, self.robotClient.status_model.world_1, self.robotClient.status_model.world_2-10, self.robotClient.status_model.world_3, self.robotClient.status_model.world_4, self.robotClient.status_model.world_5) # 黄老师给我的xyz和法向量 target_position, noraml_base = getPosition(*xyz, *uvw,rotation) log.log_message(logging.INFO, Constant.str_feed_covert_success) self.feedConfig.feedLine.take_position = Real_Position().init_position(*target_position[:3],*noraml_base) self.feedStatus = FeedStatus.FTake self.sendTargPosition(self.feedConfig.feedLine.take_position) else: log.log_message(logging.ERROR,Constant.str_feed_takePhoto_fail) print("打印日志，保存失败图像") elif self.feedStatus == FeedStatus.FTake: log.log_message(logging.INFO, Constant.str_feed_take) if self.feedConfig.feedLine.take_position != None and self.feedConfig.feedLine.take_position.compare(real_position): self.feedStatus = FeedStatus.FSafeF time.sleep(2) log.log_message(logging.INFO, Constant.str_feed_take_success) self.sendTargPosition(self.feedConfig.feedLine.mid_position) pass #打开吸嘴并返回 elif self.feedStatus == FeedStatus.FSafeF: log.log_message(logging.INFO, Constant.str_feed_mid) if self.feedConfig.feedLine.mid_position.compare(real_position): self.feedStatus = FeedStatus.FFeedP self.sendTargPosition(self.feedConfig.feedLine.feed_position) pass #吸嘴开始 elif self.feedStatus==FeedStatus.FFeedP: log.log_message(logging.INFO, Constant.str_feed_feed) if self.feedConfig.feedLine.feed_position.compare(real_position): self.feedStatus = FeedStatus.FBroken self.sendTargPosition(self.feedConfig.feedLine.broken_position) pass #破袋 elif self.feedStatus==FeedStatus.FBroken: log.log_message(logging.INFO, Constant.str_feed_broken) if self.feedConfig.feedLine.broken_position.compare(real_position): self.feedConfig.num = self.feedConfig.num-1 if self.feedConfig.num == 0: self.feedStatus=FeedStatus.FNone self.sendTargPosition(self.feedConfig.feedLine.safe_position) self.init_detection_image() log.log_message(logging.INFO, Constant.str_feed_finish) else: log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') self.feedStatus = FeedStatus.FSafeP self.sendTargPosition(self.feedConfig.feedLine.safe_position) pass def sendTargPosition(self,real_position): from Model.RobotModel import Instruction position_instruction = Instruction() position_instruction.speed = Constant.speed position_instruction.m0 = real_position.X position_instruction.m1 = real_position.Y position_instruction.m2 = real_position.Z position_instruction.m3 = real_position.U position_instruction.m4 = real_position.V position_instruction.m5 = real_position.W position_instruction.action = MoveType.WORLD.value instruction_command = CMDInstructRequest() instruction_command.instructions.append(position_instruction) request_command = instruction_command.toString() print(request_command) log_str = f'移动到位置:{"姿势直线"}：' \ f'X:{position_instruction.m0}-' \ f'Y:{position_instruction.m1}-' \ f'Z:{position_instruction.m2}-' \ f'U:{position_instruction.m3}-' \ f'V:{position_instruction.m4}-' \ f'W:{position_instruction.m5}' log.log_message(logging.INFO, log_str) self.robotClient.add_sendQuene(request_command) pass