251108问题修复
This commit is contained in:
@ -1,6 +1,8 @@
|
||||
import copy
|
||||
from dis import stack_effect
|
||||
import logging
|
||||
import math
|
||||
from pickle import FALSE
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
@ -30,6 +32,8 @@ from Model.RobotModel import Instruction
|
||||
from EMV.EMV import RelayController
|
||||
from CU.drop import DropPositionManager
|
||||
from Mv3D.CameraUtil import CameraUtil
|
||||
from Mv3D.calculate_diff2 import calculate_offset_from_image
|
||||
|
||||
class ResetStatus(Enum):
|
||||
RNone = 0
|
||||
RStart = 1
|
||||
@ -204,7 +208,7 @@ class FeedLine:
|
||||
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
|
||||
break # 假设只有一个丢包点
|
||||
|
||||
def get_drop_path(self) -> list:
|
||||
def get_drop_path(self,offsetX) -> list:
|
||||
"""获取动态扔包路径"""
|
||||
|
||||
if self.drop_manager is None:
|
||||
@ -218,8 +222,11 @@ class FeedLine:
|
||||
self.id,
|
||||
self.current_index
|
||||
)
|
||||
|
||||
if pos_model is None:
|
||||
break
|
||||
if pos_model.status==FeedStatus.FDropBag.value:
|
||||
pos_model=self.get_offset_position(pos_model,self.current_index,offsetX)
|
||||
path.append(pos_model)
|
||||
return path
|
||||
|
||||
@ -245,21 +252,51 @@ class FeedLine:
|
||||
self.start_to_take = self.feed_positions[index_start:index_take+1]
|
||||
self.feeding_to_end = self.feed_positions[index_take:]
|
||||
|
||||
def set_feeding_to_end(self):
|
||||
|
||||
def set_feeding_to_end(self,offsetX)->bool:
|
||||
"""
|
||||
设置动态扔包路径
|
||||
:param image_path: 偏移量图像路径
|
||||
"""
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
break
|
||||
index_drop=self.current_dropbag_index
|
||||
test_path = self.get_drop_path()
|
||||
test_path = self.get_drop_path(offsetX)
|
||||
self.current_index+=1
|
||||
# 将总list的drop部分，替换为动态路径
|
||||
self.feed_positions = self.feed_positions[:index_drop] + test_path
|
||||
# self.feeding_to_end = self.feed_positions[index_take:index_drop]
|
||||
self.feeding_to_end = self.feed_positions[index_take:index_drop]+test_path
|
||||
|
||||
|
||||
|
||||
# 计算偏移量
|
||||
|
||||
def get_offset_position(self,point,_current_index,offsetX):
|
||||
"""
|
||||
获取偏移后的坐标。
|
||||
"""
|
||||
_current_index=_current_index-1
|
||||
if _current_index<0:
|
||||
_current_index=0
|
||||
loc_floor=(_current_index//5)+1
|
||||
loc_bag=(_current_index%5)+1
|
||||
if loc_floor in [1,3,5]:
|
||||
if loc_bag in [1,2,3]:
|
||||
point.X=round(point.X-offsetX, 3)
|
||||
elif loc_bag==4:
|
||||
point.Y=round(point.Y-offsetX, 3)
|
||||
elif loc_bag==5:
|
||||
point.Y=round(point.Y+offsetX, 3)
|
||||
elif loc_floor in [2,4,6]:
|
||||
if loc_bag in [1,2,3]:
|
||||
point.X=round(point.X+offsetX, 3)
|
||||
elif loc_bag==4:
|
||||
point.Y=round(point.Y-offsetX, 3)
|
||||
elif loc_bag==5:
|
||||
point.Y=round(point.Y+offsetX, 3)
|
||||
return point
|
||||
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs,remain_count:int):
|
||||
#需码垛数量，如50，或30
|
||||
@ -286,6 +323,8 @@ class Feeding(QObject):
|
||||
log_signal = Signal(int,str)
|
||||
#码垛完成通知
|
||||
stack_finish_signal=Signal()
|
||||
#误差过大，通知用户
|
||||
feed_error_signal=Signal(int,str)
|
||||
|
||||
def __init__(self, robotClient: RobotClient,relay_controller:RelayController):
|
||||
super().__init__()
|
||||
@ -353,6 +392,8 @@ class Feeding(QObject):
|
||||
"""接收机器人信号通知"""
|
||||
self.take_sensor_signal = True
|
||||
|
||||
|
||||
|
||||
def run(self):
|
||||
self.catch.run()
|
||||
# 获取事件坐标
|
||||
@ -501,13 +542,41 @@ class Feeding(QObject):
|
||||
return
|
||||
time.sleep(1) # 每秒检查一次
|
||||
|
||||
loc_offsetX=0
|
||||
loc_is_next=False
|
||||
#第二次执行FeedStatus.FPhoto时，改变码垛点
|
||||
self.camera_img.save_img()
|
||||
self.feedConfig.feedLine.set_feeding_to_end()
|
||||
# self.take_photo_sigal.emit()
|
||||
self.next_position()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
|
||||
# self.feedStatus = FeedStatus.FTake
|
||||
try:
|
||||
loc_image_path=self.camera_img.save_img()
|
||||
loc_model_result = calculate_offset_from_image(loc_image_path, visualize=False)
|
||||
if loc_model_result['success']:
|
||||
#置信度
|
||||
if loc_model_result['obj_conf']>0.5:
|
||||
loc_offsetX=loc_model_result['dx_mm']
|
||||
if abs(loc_model_result['dx_mm'])<200:
|
||||
loc_is_next=True
|
||||
print(f'{loc_image_path}:料带偏移{loc_offsetX}mm')
|
||||
else:
|
||||
print(f"{loc_image_path}:料带偏移过大，报警")
|
||||
self.feed_error_signal.emit(1,f"{loc_image_path}:料带偏移过大，报警")
|
||||
else:
|
||||
print(f"{loc_image_path}:未检测到料带,置信度低")
|
||||
self.feed_error_signal.emit(2,f"{loc_image_path}:未检测到料带,置信度低")
|
||||
else:
|
||||
print(f"{loc_image_path}:计算偏移失败，视觉返回False")
|
||||
self.feed_error_signal.emit(2,f"{loc_image_path}:计算偏移失败，视觉返回False")
|
||||
# loc_is_next=False
|
||||
loc_offsetX=0
|
||||
except Exception as e:
|
||||
print(f"视频模型异常：{loc_image_path}",e)
|
||||
# loc_is_next=False
|
||||
loc_offsetX=0
|
||||
self.feed_error_signal.emit(2,"视频模型异常")
|
||||
self.log_signal.emit(logging.ERROR, f"视频模型异常：{e}")
|
||||
finally:
|
||||
if loc_is_next:
|
||||
self.feedConfig.feedLine.set_feeding_to_end(loc_offsetX)
|
||||
self.next_position()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take)
|
||||
|
||||
Reference in New Issue
Block a user