update 更新界面显示_3
This commit is contained in:
FrankCV2048
@ -1,3 +1,4 @@
|
||||
import logging
|
||||
import time
|
||||
|
||||
import cv2
|
||||
@ -8,7 +9,7 @@ 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
|
||||
@ -49,10 +50,11 @@ class Feeding():
|
||||
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("./Image/wait.png")
|
||||
self.detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
def run(self):
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
@ -69,31 +71,35 @@ class Feeding():
|
||||
# 0,
|
||||
# 0);
|
||||
|
||||
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
elif self.feedConfig.num == 0:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
if self.feedStatus==FeedStatus.FNone:
|
||||
pass
|
||||
if self.feedStatus==FeedStatus.FNone or self.pause:
|
||||
return
|
||||
elif self.feedStatus==FeedStatus.FStart:
|
||||
log.log_message(logging.INFO,Constant.str_feed_start)
|
||||
if self.feedConfig.num != 0:
|
||||
self.feedStatus = FeedStatus.FSafeP
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
#print(request_command)
|
||||
|
||||
# print(request_command)
|
||||
pass
|
||||
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 = 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,
|
||||
@ -104,41 +110,51 @@ class Feeding():
|
||||
self.robotClient.status_model.world_5)
|
||||
|
||||
# 黄老师给我的xyz和法向量
|
||||
|
||||
target_position, noraml_base = getPosition(*xyz, *uvw,rotation)
|
||||
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(1)
|
||||
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.broken_position)
|
||||
pass #破袋
|
||||
|
||||
elif self.feedStatus==FeedStatus.FBroken:
|
||||
log.log_message(logging.INFO, Constant.str_feed_broken)
|
||||
if self.feedConfig.feedLine.broken_position.compare(real_position):
|
||||
self.feedConfig.num = self.feedConfig.num-1
|
||||
if self.feedConfig.num == 0:
|
||||
self.feedStatus=FeedStatus.FNone
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
self.init_detection_image()
|
||||
log.log_message(logging.INFO, Constant.str_feed_finish)
|
||||
else:
|
||||
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)
|
||||
pass
|
||||
@ -158,6 +174,16 @@ class Feeding():
|
||||
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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user