# 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 # CureStart = 1 # 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.start2take_pos_index[self.start2take_pos_index] # return pos # def get_current_start_position(self): # pos = self.origin2start_pos_index[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.origin2start_pos_index[self.origin2start_pos_index] # if reverse: # self.origin2start_pos_index -= 1 # if self.origin2start_pos_index < 0: # self.origin2start_pos_index = len(self.origin2start_pos) - 1 # else: # self.origin2start_pos_index += 1 # if self.origin2start_pos_index >= len(self.origin2start_pos): # self.origin2start_pos_index = 0 # # return pos # # def get_next_take_position(self,reverse:bool=False): # pos = self.start2take_pos_index[self.start2take_pos_index] # if reverse: # self.start2take_pos_index -= 1 # if self.start2take_pos_index < 0: # self.start2take_pos_index = len(self.start2take_pos) - 1 # else: # self.start2take_pos_index += 1 # if self.start2take_pos_index >= len(self.start2take_pos): # 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: # 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: # self.feeding_to_end[i].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.FStart: # index_start = i # break # for i in range(len(self.feed_positions)): # if self.feed_positions[i].status == FeedStatus.FPhoto: # 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 # if self.is_reverse and self.robotClient.get_origin_position().compare(real_position): # self.feedStatus = FeedStatus.FNone # self.is_reverse = False # # 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.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() != None: # if self.feedConfig.feedLine.get_take_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) # # # # elif self.feedStatus == FeedStatus.FBroken1: # log.log_message(logging.INFO, Constant.str_feed_broken) # if self.get_current_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().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().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().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.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}' # # 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 = start_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone # if start_pos.lineType == LineType.CureMid: # start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse) # self.sendTargPosition(real_position=start_pos.position, move_type=MoveType.Cure, real_position1=start_pos1.position) # else: # self.sendTargPosition(start_pos.position) # pass # # def next_take(self,reverse=False): # take_pos = self.feedConfig.feedLine.get_next_take_position(reverse) # self.feedStatus = take_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone # if take_pos.lineType == LineType.CureMid: # take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse) # self.sendTargPosition(real_position=take_pos.position, move_type=MoveType.Cure, real_position1=take_pos1.position) # else: # self.sendTargPosition(take_pos.position) # pass # # def next_Feed(self,reverse=False): # feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse) # self.feedStatus = feed_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone # if feed_pos.lineType == LineType.CureMid: # feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse) # self.sendTargPosition(real_position=feed_pos.position, move_type=MoveType.Cure, real_position1=feed_pos1.position) # else: # self.sendTargPosition(feed_pos.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 #