☢️ update 初步更新投料框架
This commit is contained in:
FrankCV2048
10
CU/Catch.py
Normal file
10
CU/Catch.py
Normal file
@ -0,0 +1,10 @@
|
||||
class catch_jaw:
|
||||
def __init__(self, x, y, width, height, angle, color):
|
||||
pass
|
||||
|
||||
def run(self):
|
||||
pass
|
||||
|
||||
|
||||
def take_bag(self):
|
||||
return True
|
||||
@ -32,6 +32,8 @@ class FeedStatus(IntEnum):
|
||||
|
||||
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
|
||||
@ -68,6 +70,7 @@ class Feeding :
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
|
||||
pass
|
||||
|
||||
def init_detection_image(self):
|
||||
|
||||
324
CU/Feeding_C.py
Normal file
324
CU/Feeding_C.py
Normal file
@ -0,0 +1,324 @@
|
||||
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
|
||||
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
|
||||
|
||||
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.pos_index = 0
|
||||
self.name = name
|
||||
self.id = id
|
||||
|
||||
def get_current_position(self):
|
||||
pos = self.feed_positions[self.pos_index]
|
||||
return pos
|
||||
def get_next_position(self):
|
||||
pos = self.feed_positions[self.pos_index]
|
||||
self.pos_index += 1
|
||||
return pos
|
||||
def get_take_position(self):
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FTake:
|
||||
return self.feed_positions[i]
|
||||
|
||||
def set_take_position(self,position:Real_Position):
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FTake:
|
||||
self.feed_positions[i].position = position
|
||||
|
||||
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()
|
||||
|
||||
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 if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
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.next_target()
|
||||
# if == 原点 继续判断
|
||||
# else:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_safe_error_msgbox)
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
log.log_message(logging.INFO, Constant.str_feed_mid)
|
||||
feed_pos = self.feedConfig.feedLine.get_current_position()
|
||||
if feed_pos.position.compare(real_position):
|
||||
self.next_target()
|
||||
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
log.log_message(logging.INFO, Constant.str_feed_photo)
|
||||
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 - 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]]
|
||||
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_target()
|
||||
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_target()
|
||||
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.get_current_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(self.robotClient.time_delay_take)
|
||||
log.log_message(logging.INFO, Constant.str_feed_take_success)
|
||||
self.next_target()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
log.log_message(logging.INFO, Constant.str_feed_broken)
|
||||
if self.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
self.next_target()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
log.log_message(logging.INFO, Constant.str_feed_broken)
|
||||
if self.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
self.next_target()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
log.log_message(logging.INFO, Constant.str_feed_shake)
|
||||
if self.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
# TODO 震动方案
|
||||
time.sleep(self.robotClient.time_delay_shake)
|
||||
self.next_target()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
log.log_message(logging.INFO, Constant.str_feed_drop)
|
||||
|
||||
if self.feedConfig.feedLine.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)
|
||||
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 and self.feedConfig.feedLine.pos_index == 0:
|
||||
log.log_message(logging.INFO, Constant.str_feed_finish)
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.init_detection_image()
|
||||
|
||||
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):
|
||||
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()
|
||||
|
||||
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_target(self):
|
||||
feed_pos = self.feedConfig.feedLine.get_next_position()
|
||||
self.feedStatus = feed_pos.status
|
||||
self.sendTargPosition(feed_pos.position)
|
||||
def safe_check_columns(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
def safe_check_person(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
Reference in New Issue
Block a user