284 lines
13 KiB
Python
284 lines
13 KiB
Python
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)
|
||
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
|
||
|