252 lines
11 KiB
Python
252 lines
11 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,photo_position:Real_Position,mid_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,drop_bag_position:Real_Position,zip_bag_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.broken1_position = broken1_position
|
||
self.broken2_position = broken2_position
|
||
self.drop_bag_position = drop_bag_position
|
||
self.zip_bag_position = zip_bag_position
|
||
self.take_position = None
|
||
self.name = name
|
||
self.id=id
|
||
|
||
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)
|
||
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)
|
||
if self.feedConfig.feedLine.photo_position.compare(real_position):
|
||
code, img, xyz, uvw, mng = 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,*mng)
|
||
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.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):
|
||
current_position = Real_Position().init_position(0,0,0,0,0,0)
|
||
current_position.X = self.robotClient.status_model.axis_0
|
||
current_position.Y = self.robotClient.status_model.axis_1
|
||
current_position.Z = self.robotClient.status_model.axis_2
|
||
current_position.U = self.robotClient.status_model.axis_3
|
||
current_position.V = self.robotClient.status_model.axis_4
|
||
current_position.W = self.robotClient.status_model.axis_5
|
||
for i in range(8):
|
||
if (i % 2 == 0) ^ (i >= 3): ## 过一半取消
|
||
c1 = current_position.U - 5
|
||
self.sendTargPosition(c1, MoveType.AXIS, speed=Constant.shake_speed)
|
||
else:
|
||
c1 = current_position.U + 5
|
||
self.sendTargPosition(c1, MoveType.AXIS, speed=Constant.shake_speed)
|
||
if True: # 延迟判断，如果最后点位延迟1s，则认为阶段完成
|
||
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):
|
||
if self.feedConfig.num - 1 != 0:
|
||
self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
|
||
else:
|
||
self.sendTargPosition(self.feedConfig.feedLine.zip_bag_position)
|
||
pass
|
||
if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
|
||
# TODO 松开吸嘴
|
||
self.feedConfig.num = self.feedConfig.num - 1
|
||
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)
|
||
|
||
|
||
if self.feedConfig.feedLine.zip_bag_position.compare(real_position):
|
||
log.log_message(logging.INFO, Constant.str_feed_zip_bag)
|
||
# TODO 松开吸嘴 是否需要判断放没放开不
|
||
self.feedStatus = FeedStatus.FNone
|
||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||
self.init_detection_image()
|
||
log.log_message(logging.INFO, Constant.str_feed_finish)
|
||
|
||
|
||
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
|
||
|