米厂码垛修改
This commit is contained in:
177
CU/Feeding.py
177
CU/Feeding.py
@ -1,4 +1,5 @@
|
||||
import copy
|
||||
from dis import stack_effect
|
||||
import logging
|
||||
import random
|
||||
import threading
|
||||
@ -51,9 +52,13 @@ class FeedStatus(IntEnum):
|
||||
FStartReverse = 13
|
||||
|
||||
class LineType(Enum):
|
||||
#直线
|
||||
Straight = 0
|
||||
#曲线中间点
|
||||
CureMid = 2
|
||||
#曲线终点
|
||||
CureEnd = 3
|
||||
#关节（自由路径）
|
||||
WORLD = 4
|
||||
|
||||
|
||||
@ -69,19 +74,25 @@ class FeedPosition:
|
||||
self.position = position
|
||||
|
||||
class FeedLine:
|
||||
def __init__(self, id, name, feed_positions:list):
|
||||
def __init__(self, id, name, feed_positions:list,remain_count:int):
|
||||
self.feed_positions = copy.deepcopy(feed_positions)
|
||||
|
||||
self.feeding2end_pos_index = 0
|
||||
self.origin2start_pos_index = 0
|
||||
self.start2take_pos_index = 0
|
||||
self.name = name
|
||||
self.id = id
|
||||
self.drop_manager = DropPositionManager("CU/drop.ini")
|
||||
# 初始化各个阶段的位置列表
|
||||
self.feeding_to_end = []
|
||||
|
||||
# --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
|
||||
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
|
||||
self.drop_point_list = []
|
||||
self.current_index = 1
|
||||
#读取码垛点位置
|
||||
self.current_index = remain_count+1
|
||||
#记录feed_positions当前扔包点索引
|
||||
self.current_dropbag_index=0
|
||||
|
||||
def get_current_index(self):
|
||||
return self.current_index
|
||||
@ -198,6 +209,8 @@ class FeedLine:
|
||||
|
||||
path = []
|
||||
while True:
|
||||
#current_index,当前扔包点id
|
||||
#drop.ini定义point1,point2,current_index对应后面的数字
|
||||
pos_model = self.drop_manager.get_next_drop_position(
|
||||
self.id,
|
||||
self.current_index
|
||||
@ -217,12 +230,13 @@ class FeedLine:
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
|
||||
# 开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FDropBag.value:
|
||||
index_drop = i
|
||||
|
||||
# LineID = self.id
|
||||
#开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
|
||||
#记录feed_positions扔包点初始位置,后续动态增加路径仍从此获取
|
||||
if self.current_dropbag_index==0:
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FDropBag.value:
|
||||
self.current_dropbag_index = i
|
||||
break
|
||||
|
||||
# 开始插入动态扔包中间点
|
||||
|
||||
@ -230,16 +244,16 @@ class FeedLine:
|
||||
|
||||
# 开始插入复位中间点
|
||||
|
||||
test_path = self.get_drop_path()
|
||||
# test_path = self.get_drop_path()
|
||||
|
||||
self.origin_to_start = self.feed_positions[: index_start+1]
|
||||
self.start_to_take = self.feed_positions[index_start:index_take+1]
|
||||
|
||||
# 将总list的drop部分，替换为动态路径
|
||||
self.feed_positions = self.feed_positions[:index_drop] + test_path
|
||||
self.feeding_to_end = self.feed_positions[index_take:index_drop]
|
||||
|
||||
print(self.feed_positions)
|
||||
# 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:]
|
||||
# print(self.feed_positions)
|
||||
|
||||
# for i in range(len(self.feeding_to_end)): #插入动态中间点
|
||||
# if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
@ -252,15 +266,32 @@ class FeedLine:
|
||||
# self.feeding_to_end.insert(i+2, after_position_model)
|
||||
# break
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def set_feeding_to_end(self):
|
||||
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
break
|
||||
# 开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
|
||||
# for i in range(len(self.feed_positions)):
|
||||
# if self.feed_positions[i].status == FeedStatus.FDropBag.value:
|
||||
# index_drop = i
|
||||
# break
|
||||
index_drop=self.current_dropbag_index
|
||||
test_path = self.get_drop_path()
|
||||
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
|
||||
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs,remain_count:int):
|
||||
#需码垛数量，如50，或30
|
||||
self.num = num
|
||||
#已经码垛数量
|
||||
self.remain_count=remain_count
|
||||
self.feedLine = feedLine
|
||||
self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
|
||||
|
||||
@ -279,6 +310,8 @@ class Feeding(QObject):
|
||||
take_no_photo_sigal = Signal()
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
log_signal = Signal(int,str)
|
||||
#码垛完成通知
|
||||
stack_finish_signal=Signal()
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
super().__init__()
|
||||
self.feedConfig = None
|
||||
@ -326,7 +359,8 @@ class Feeding(QObject):
|
||||
def close_feed(self):
|
||||
self.is_detected = False
|
||||
self.detect_thread.join()
|
||||
self.detect.detection.release()
|
||||
if self.detect.detection:
|
||||
self.detect.detection.release()
|
||||
|
||||
def run_detect(self):
|
||||
while self.is_detected:
|
||||
@ -337,13 +371,19 @@ class Feeding(QObject):
|
||||
self.catch.run()
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
real_position.init_position_joint_and_world(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)
|
||||
self.robotClient.status_model.world_5,
|
||||
self.robotClient.status_model.axis_0,
|
||||
self.robotClient.status_model.axis_1,
|
||||
self.robotClient.status_model.axis_2,
|
||||
self.robotClient.status_model.axis_3,
|
||||
self.robotClient.status_model.axis_4,
|
||||
self.robotClient.status_model.axis_5
|
||||
)
|
||||
# real_position.init_position(0,
|
||||
# 0,
|
||||
# 0,
|
||||
@ -359,8 +399,9 @@ class Feeding(QObject):
|
||||
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position,is_action=True):
|
||||
|
||||
#
|
||||
if self.feedConfig !=None and self.is_reverse and real_position.compare(self.robotClient.origin_position,is_action=True):
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
|
||||
@ -386,7 +427,7 @@ class Feeding(QObject):
|
||||
else:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
|
||||
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
|
||||
if real_position.compare(self.feedConfig.feedLine.start_to_take[0].get_position()):
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FStart:
|
||||
@ -394,7 +435,7 @@ class Feeding(QObject):
|
||||
self.relay_controller.open(conveyor2=True)#开电机
|
||||
#self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
|
||||
#self.sensor2_thread.start()
|
||||
if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse:
|
||||
if not real_position.compare(self.robotClient.origin_position,is_action=True) and not self.is_reverse:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
|
||||
self.need_origin_signal.emit(Constant.str_feed_start_error)
|
||||
@ -412,7 +453,7 @@ class Feeding(QObject):
|
||||
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
feed_pos = self.get_current_position(self.is_reverse)
|
||||
if feed_pos.get_position().compare(real_position):
|
||||
if real_position.compare(feed_pos.get_position()):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
|
||||
self.next_position(self.is_reverse)
|
||||
# 增加计数器逻辑
|
||||
@ -428,18 +469,26 @@ class Feeding(QObject):
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
if self.feedConfig.num == 0:
|
||||
#码垛的数量和配置的数量一致时
|
||||
if self.feedConfig.remain_count >=self.feedConfig.num:
|
||||
#关闭，暂停
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_finish)
|
||||
self.is_reverse = True
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2
|
||||
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
#码垛数量重置
|
||||
self.feedConfig.remain_count=0
|
||||
self.next_position(self.is_reverse)
|
||||
#码垛完成信号通知
|
||||
self.stack_finish_signal.emit()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
|
||||
return
|
||||
|
||||
if not Constant.Debug:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
|
||||
if(self.feed_Mid_Status == FeedMidStatus.FMid_Feed):
|
||||
self.feedConfig.feedLine.set_feeding_to_end()
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Feed
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
self.feedConfig.feedLine.set_take_position(self.detect.detect_position, 0)
|
||||
@ -456,10 +505,9 @@ class Feeding(QObject):
|
||||
else:
|
||||
self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...")
|
||||
time.sleep(1) # 每秒检查一次
|
||||
|
||||
|
||||
self.next_position()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
|
||||
|
||||
# self.feedStatus = FeedStatus.FTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
@ -468,7 +516,7 @@ class Feeding(QObject):
|
||||
if not take_position or not take_position.get_position():
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
return
|
||||
if not take_position.get_position().compare(real_position, is_action=True):
|
||||
if not real_position.compare(take_position.get_position(), is_action=True):
|
||||
self.log_signal.emit(logging.INFO, "机器人尚未到达抓料点位")
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, "机器人已到达抓料点位")
|
||||
@ -500,21 +548,19 @@ class Feeding(QObject):
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
if real_position.compare(self.get_current_position().get_position()):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropMid:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
else:
|
||||
if real_position.compare(self.get_current_position().get_position(),is_action=True):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop_mid)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
if real_position.compare(self.get_current_position().get_position(),is_action=True):
|
||||
# TODO 震动方案
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
@ -537,13 +583,14 @@ class Feeding(QObject):
|
||||
# get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
|
||||
# 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
|
||||
# 由 feeding2end_pos_index 指向的点。
|
||||
if self.get_current_position().get_position().compare(real_position, is_action=True):
|
||||
if real_position.compare(self.get_current_position().get_position(),is_action=True):
|
||||
# 2. 记录日志：已到达投料点
|
||||
#if not self.sensor2_ready:
|
||||
#self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
|
||||
#self.relay_controller.open(conveyor2=True)
|
||||
|
||||
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
|
||||
|
||||
# 3. 与 Catch 模块进行状态交互来驱动投料动作
|
||||
# a. 初始状态 (CNone): 触发投料动作
|
||||
|
||||
@ -567,15 +614,21 @@ class Feeding(QObject):
|
||||
# (后续增加) 视觉确认: 拍照确认袋子已放置
|
||||
# self.detection.get_position(...)
|
||||
# self.feedConfig.feedLine.set_take_position(...)
|
||||
# 4. 更新业务逻辑：减少剩余袋数
|
||||
self.feedConfig.num = self.feedConfig.num - 1
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
|
||||
# 码垛数量增加
|
||||
self.feedConfig.remain_count = self.feedConfig.remain_count + 1
|
||||
# self.feedConfig.num = self.feedConfig.num - 1
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.remain_count}')
|
||||
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
|
||||
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
|
||||
# 调用 next_Feed()。
|
||||
self.next_position()
|
||||
elif self.feedStatus == FeedStatus.FDropReset:
|
||||
self.next_position()
|
||||
elif self.feedStatus == FeedStatus.FDropReset:
|
||||
if real_position.compare(self.get_current_position().get_position()):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop_reset)
|
||||
self.next_position()
|
||||
|
||||
|
||||
|
||||
elif self.feedStatus == None:
|
||||
print(self.feedStatus)
|
||||
@ -583,12 +636,18 @@ class Feeding(QObject):
|
||||
|
||||
def run_reset(self):
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
real_position.init_position_joint_and_world(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)
|
||||
self.robotClient.status_model.world_5,
|
||||
self.robotClient.status_model.axis_0,
|
||||
self.robotClient.status_model.axis_1,
|
||||
self.robotClient.status_model.axis_2,
|
||||
self.robotClient.status_model.axis_3,
|
||||
self.robotClient.status_model.axis_4,
|
||||
self.robotClient.status_model.axis_5)
|
||||
if self.reset_status == ResetStatus.RNone:
|
||||
return
|
||||
|
||||
@ -601,7 +660,7 @@ class Feeding(QObject):
|
||||
self.pos_near_index = -1
|
||||
self.reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position,is_action=True):
|
||||
if real_position.compare(pos_model.get_position(),is_action=True):
|
||||
self.pos_index = index
|
||||
break
|
||||
|
||||
@ -639,6 +698,11 @@ class Feeding(QObject):
|
||||
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
elif pos_model.lineType==LineType.WORLD.value:
|
||||
#关节移动
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
|
||||
self.current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model
|
||||
@ -654,12 +718,18 @@ class Feeding(QObject):
|
||||
|
||||
self.run_reverse = True
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
real_position.init_position_joint_and_world(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)
|
||||
self.robotClient.status_model.world_5,
|
||||
self.robotClient.status_model.axis_0,
|
||||
self.robotClient.status_model.axis_1,
|
||||
self.robotClient.status_model.axis_2,
|
||||
self.robotClient.status_model.axis_3,
|
||||
self.robotClient.status_model.axis_4,
|
||||
self.robotClient.status_model.axis_5)
|
||||
if self.feedConfig == None: return
|
||||
start_index = -1
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
@ -669,7 +739,7 @@ class Feeding(QObject):
|
||||
pos_near_index = -1
|
||||
reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position):
|
||||
if real_position.compare(pos_model.get_position()):
|
||||
pos_index = index
|
||||
break
|
||||
|
||||
@ -769,9 +839,8 @@ class Feeding(QObject):
|
||||
|
||||
|
||||
try:
|
||||
print('fuck1',log_str)
|
||||
self.log_signal.emit(logging.INFO, log_str)
|
||||
print('fuck2',log_str)
|
||||
print(log_str)
|
||||
except:
|
||||
return
|
||||
print(request_command)
|
||||
@ -843,8 +912,8 @@ class Feeding(QObject):
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
elif feed_pos.lineType == LineType.WORLD.value:
|
||||
# 暂时不考虑世界坐标到关节坐标的转换，强行不判断接近
|
||||
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
# feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
# self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user