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 FCheck = 2 FSafeP = 3 FPhoto = 4 FTake = 5 FSafeF = 6 FFeedP = 7 FBroken = 8 FCut = 9 FShake = 10 FDropBag = 11 FFinished = 12 class FeedLine: def __init__(self,id,name,safe_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,shake_position:Real_Position,drop_bag_position:Real_Position): self.safe_position = safe_position self.broken1_position = broken1_position self.broken2_position = broken2_position self.shake_position = shake_position self.drop_bag_position = drop_bag_position self.take_position = None self.name = name self.id=id 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 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) self.feedStatus = FeedStatus.FCheck pass 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.feedStatus = FeedStatus.FSafeP self.sendTargPosition(self.feedConfig.feedLine.safe_position) 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) detect_pos_list = [] for pos in self.feedConfig.photo_locs: self.sendTargPosition(pos) while not pos.compare(real_position): #可以优化 TODO 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 - 10, #TODO 这个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, *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]] self.feedConfig.feedLine.take_position = detect_pos_list[0] log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish) else: self.feedConfig.feedLine.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: self.feedConfig.feedLine.take_position = detect_pos_list[0] log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line) else: self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index] log.log_message(logging.INFO, Constant.str_feed_takePhoto_line) self.feedStatus = FeedStatus.FTake self.sendTargPosition(self.feedConfig.feedLine.take_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.take_position != None and self.feedConfig.feedLine.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 检测是否通 不然报警 time.sleep(2) self.feedStatus = FeedStatus.FSafeF log.log_message(logging.INFO, Constant.str_feed_take_success) self.sendTargPosition(self.feedConfig.feedLine.safe_position) pass elif self.feedStatus == FeedStatus.FSafeF: log.log_message(logging.INFO, Constant.str_feed_mid) if self.feedConfig.feedLine.safe_position.compare(real_position): self.feedStatus = FeedStatus.FBroken self.sendTargPosition(self.feedConfig.feedLine.broken1_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.broken1_position) # pass #破袋 elif self.feedStatus==FeedStatus.FBroken: log.log_message(logging.INFO, Constant.str_feed_broken) if self.feedConfig.feedLine.broken1_position.compare(real_position): self.sendTargPosition(self.feedConfig.feedLine.broken2_position) if self.feedConfig.feedLine.broken2_position.compare(real_position): self.sendTargPosition(self.feedConfig.feedLine.shake_position) if self.feedConfig.feedLine.shake_position.compare(real_position): # 延迟判断，如果最后点位延迟1s，则认为阶段完成 # TODO 震动方案 time.sleep(2) self.feedStatus = FeedStatus.FDropBag self.sendTargPosition(self.feedConfig.feedLine.safe_position) elif self.feedStatus == FeedStatus.FDropBag: log.log_message(logging.INFO, Constant.str_feed_drop) if self.feedConfig.feedLine.safe_position.compare(real_position): self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position) pass if self.feedConfig.feedLine.drop_bag_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(2) self.feedConfig.num = self.feedConfig.num - 1 log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') if self.feedConfig.num == 0: # 投料次数为0，直接返回 log.log_message(logging.INFO, Constant.str_feed_finish) self.feedStatus = FeedStatus.FNone self.sendTargPosition(self.feedConfig.feedLine.safe_position) self.init_detection_image() else: self.feedStatus = FeedStatus.FSafeP self.sendTargPosition(self.feedConfig.feedLine.safe_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): from Model.RobotModel import Instruction 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()) pass def sendTargPosition(self,real_position,move_type:MoveType=MoveType.WORLD ,speed = Constant.speed): from Model.RobotModel import Instruction 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 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 def safe_check_columns(self): return True pass def safe_check_person(self): return True pass