import logging import time import cv2 from PyQt5.QtWidgets import QMessageBox import Constant import Expection 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 Util.util_time import CRisOrFall from Vision.camera_coordinate_dete import Detection from Util.util_log import log from Model.RobotModel import Instruction 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 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 = feed_positions self.feeding2end_pos_index = 0 self.origin2start_pos_index = 0 self.start2take_pos_index = 0 self.name = name self.id = id self.get_position_list() def get_current_feed_position(self): pos = self.feeding_to_end[self.feeding2end_pos_index] return pos def get_current_take_position(self): pos = self.start_to_take[self.start2take_pos_index] return pos def get_current_start_position(self): pos = self.origin_to_start[self.origin2start_pos_index] 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): for i in range(len(self.feeding_to_end)): if self.feeding_to_end[i].status == FeedStatus.FTake.value: self.feeding_to_end[i].set_position(position) 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:] 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: 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 self.cRis_photo = CRisOrFall() self.feed_Mid_Status = FeedMidStatus.FMid_Start self.is_reverse = 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 and self.feedStatus!=FeedStatus.FReverse: self.feedStatus = FeedStatus.FNone if self.feedStatus == FeedStatus.FNone or self.pause: return elif self.feedStatus == FeedStatus.FCheck: log.log_message(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_safe_error_msgbox) if self.is_reverse: self.feed_Mid_Status = FeedMidStatus.FMid_Start else: self.feed_Mid_Status = FeedMidStatus.FMid_Take self.next_position(self.is_reverse) elif self.feedStatus == FeedStatus.FStart: log.log_message(logging.INFO, Constant.str_feed_start) if not self.robotClient.get_origin_position().compare(real_position) and not self.is_reverse: QMessageBox.information(None, "提示", Constant.str_feed_start_error) self.feedStatus = FeedStatus.FNone return if self.is_reverse and self.robotClient.get_origin_position().compare(real_position): self.feedStatus = FeedStatus.FNone self.is_reverse = False return # TODO 获取目标位置 self.feedConfig.feedLine.set_take_position(real_position) # self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, # save_img_point=0, Height_reduce=30, width_reduce=30) self.feed_Mid_Status = FeedMidStatus.FMid_Start self.next_position(self.is_reverse) elif self.feedStatus == FeedStatus.FMid: log.log_message(logging.INFO, Constant.str_feed_mid) feed_pos = self.get_current_position() if feed_pos.get_position().compare(real_position): self.next_position(self.is_reverse) elif self.feedStatus == FeedStatus.FPhoto: log.log_message(logging.INFO, Constant.str_feed_photo) if self.feedConfig.num == 0: log.log_message(logging.INFO, Constant.str_feed_finish) self.is_reverse = True self.FeedMid_Status = FeedMidStatus.FMid_Take self.next_position(self.is_reverse) return if self.robotClient.type_detection == DetectType.EyeOutHand: self.feed_Mid_Status = FeedMidStatus.FMid_Feed self.next_position() return detect_pos_list = [] if not Constant.Debug: try: from Util.util_time import CRisOrFall if self.cRis_photo.Q(self.error_photo_count >= 5, True): QMessageBox.information(None, "提示", Constant.str_feed_photo_error_msgbox) self.error_photo_count = 0 log.log_message(logging.INFO, Constant.str_feed_photo_confirm) # TODO 返回初始状态 for pos in self.feedConfig.photo_locs: self.sendTargPosition(pos) while not pos.compare(real_position): # 可以优化 TODO if self.feedStatus == FeedStatus.FNone or not self.pause: return time.sleep(0.1) code, img, xyz, uvw, mng = self.detection.get_position() # 检测结果 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, 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, *mng) detect_pos_list.append(Real_Position().init_position(*target_position[:3], *noraml_base)) log.log_message(logging.INFO, Constant.str_feed_takePhoto_success) else: log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail + real_position.to_string()) z_diff, max_z_index = (lambda pts: ( max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z, pts.index(max(pts, key=lambda p: p.Z)) ))(detect_pos_list) if len(self.feedConfig.photo_locs) == 5: if z_diff < Constant.bag_height and len( detect_pos_list) == 3: # 第一次检测到没有高度差距开始三列拍照 TODO 保留全部的开关 # 拍照位置从五个变为三个 self.feedConfig.photo_locs = [detect_pos_list[0], detect_pos_list[2], detect_pos_list[4]] take_position = detect_pos_list[0] log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish) else: take_position = detect_pos_list[max_z_index] log.log_message(logging.INFO, Constant.str_feed_takePhoto_front) else: if z_diff < Constant.bag_height: take_position = detect_pos_list[0] log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line) else: take_position = detect_pos_list[max_z_index] log.log_message(logging.INFO, Constant.str_feed_takePhoto_line) self.feedConfig.feedLine.set_take_position(take_position) ##TODO 检查有没有异常 self.next_position() except: log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail) self.error_photo_count += 1 else: self.feedConfig.feedLine.set_take_position(real_position) ##TODO 检查有没有异常 self.next_position() log.log_message(logging.INFO, Constant.str_feed_takePhoto_move) elif self.feedStatus == FeedStatus.FTake: log.log_message(logging.INFO, Constant.str_feed_take) if self.feedConfig.feedLine.get_take_position().get_position() != None: if self.feedConfig.feedLine.get_take_position().get_position().compare(real_position): # 打开吸嘴并返回 self.sendIOControl(self.robotClient.con_ios[0], 1) self.sendIOControl(self.robotClient.con_ios[1], 1) self.sendIOControl(self.robotClient.con_ios[2], 1) # TODO 检测是否通 不然报警 self.feedConfig.feedLine.set_take_position(None) time.sleep(self.robotClient.time_delay_take) log.log_message(logging.INFO, Constant.str_feed_take_success) self.next_position() elif self.feedStatus == FeedStatus.FBroken1: log.log_message(logging.INFO, Constant.str_feed_broken) if self.get_current_position().get_position().compare(real_position): self.next_position() elif self.feedStatus == FeedStatus.FBroken2: log.log_message(logging.INFO, Constant.str_feed_broken) if self.get_current_position().get_position().compare(real_position): self.next_position() elif self.feedStatus == FeedStatus.FShake: log.log_message(logging.INFO, Constant.str_feed_shake) if self.get_current_position().get_position().compare(real_position): # TODO 震动方案 time.sleep(self.robotClient.time_delay_shake) self.next_position() elif self.feedStatus == FeedStatus.FDropBag: log.log_message(logging.INFO, Constant.str_feed_drop) if self.get_current_position().get_position().compare(real_position): self.sendIOControl(self.robotClient.con_ios[0], 0) self.sendIOControl(self.robotClient.con_ios[1], 0) self.sendIOControl(self.robotClient.con_ios[2], 0) # TODO 检测是否断 不然报警 time.sleep(self.robotClient.time_delay_put) # TODO 获取目标位置 # self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, save_img_point=0, Height_reduce=30, width_reduce=30) self.feedConfig.feedLine.set_take_position(real_position) self.feedConfig.num = self.feedConfig.num - 1 log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') self.next_position() def send_emergency_sound(self): self.sendIOControl(Constant.IO_EmergencyPoint, 1) def send_emergency_stop(self): self.sendIOControl(Constant.IO_EmergencyPoint, 0) def sendIOControl(self, IO_bit, IO_Status: int): IO_command = CMDInstructRequest() io_instruction = Instruction() io_instruction.IO = True io_instruction.io_status = IO_Status io_instruction.point = IO_bit # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]} IO_command.dsID = 'HCRemoteCommand' IO_command.instructions.append(io_instruction) self.robotClient.add_sendQuene(IO_command.toString()) log.log_message(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}') pass def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed,real_position1=None): position_instruction = Instruction() position_instruction.speed = 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 = move_type.value if position_instruction.action == 17: position_instruction.m0_p = real_position1.X position_instruction.m1_p = real_position1.Y position_instruction.m2_p = real_position1.Z position_instruction.m3_p = real_position1.U position_instruction.m4_p = real_position1.V position_instruction.m5_p = real_position1.W instruction_command = CMDInstructRequest() instruction_command.instructions.append(position_instruction) request_command = instruction_command.toString() 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}' print(log_str) try: log.log_message(logging.INFO, log_str) except: print("error") self.robotClient.add_sendQuene(request_command) pass def next_start(self,reverse=False): start_pos = self.feedConfig.feedLine.get_next_start_position(reverse) self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone if start_pos.lineType == LineType.CureMid.value: start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse) self.sendTargPosition(real_position=start_pos.get_position(), move_type=MoveType.Cure, real_position1=start_pos1.get_position()) else: self.sendTargPosition(real_position=start_pos.get_position()) pass def next_take(self,reverse=False): take_pos = self.feedConfig.feedLine.get_next_take_position(reverse) self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone if take_pos.lineType == LineType.CureMid.value: take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse) self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position()) else: self.sendTargPosition(real_position=take_pos.get_position()) pass def next_Feed(self,reverse=False): feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse) self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone if feed_pos.lineType == LineType.CureMid.value: feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse) self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position()) else: self.sendTargPosition(real_position=feed_pos.get_position()) def get_current_position(self): if self.feed_Mid_Status == FeedMidStatus.FMid_Start: return self.feedConfig.feedLine.get_current_start_position() elif self.feed_Mid_Status == FeedMidStatus.FMid_Take: return self.feedConfig.feedLine.get_current_take_position() elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed: return self.feedConfig.feedLine.get_current_feed_position() def next_position(self,reverse=False): if self.feed_Mid_Status == FeedMidStatus.FMid_Start: self.next_start(reverse) elif self.feed_Mid_Status == FeedMidStatus.FMid_Take: self.next_take(reverse) elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed: self.next_Feed(reverse) def safe_check_columns(self): return True pass def safe_check_person(self): return True pass