修改drop，获取丢包路径
This commit is contained in:
184
CU/Feeding.py
184
CU/Feeding.py
@ -42,12 +42,13 @@ class FeedStatus(IntEnum):
|
||||
FPhoto = 4
|
||||
FTake = 5
|
||||
FBroken1 = 6
|
||||
FBroken2 =7
|
||||
FDropMid = 7 # 暂时替换为扔包中间点
|
||||
FShake = 8
|
||||
FDropBag = 9
|
||||
FFinished = 10
|
||||
FReverse = 11
|
||||
FStartReverse = 12
|
||||
FDropReset = 10
|
||||
FFinished = 11
|
||||
FReverse = 12
|
||||
FStartReverse = 13
|
||||
|
||||
class LineType(Enum):
|
||||
Straight = 0
|
||||
@ -60,6 +61,7 @@ class FeedMidStatus(Enum):
|
||||
FMid_Start = 1
|
||||
FMid_Take = 2
|
||||
FMid_Feed= 3
|
||||
FMid_Reverse = 4
|
||||
|
||||
class FeedPosition:
|
||||
def __init__(self,status:FeedStatus,position:Real_Position):
|
||||
@ -74,10 +76,18 @@ class FeedLine:
|
||||
self.start2take_pos_index = 0
|
||||
self.name = name
|
||||
self.id = id
|
||||
self.drop_manager = DropPositionManager("CU/drop.ini")
|
||||
|
||||
# --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
|
||||
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
|
||||
self.drop_point_list = []
|
||||
self.current_index = 1
|
||||
|
||||
def get_current_index(self):
|
||||
return self.current_index
|
||||
|
||||
def set_current_index(self, index):
|
||||
self.current_index = index
|
||||
|
||||
def get_current_feed_position(self,is_reverse):
|
||||
pos = self.feeding_to_end[ (self.feeding2end_pos_index+1)%len(self.feeding_to_end) if is_reverse else self.feeding2end_pos_index-1]
|
||||
@ -166,6 +176,7 @@ class FeedLine:
|
||||
print("Warning: No position after FTake to update.")
|
||||
break # 抓料点暂时就一个
|
||||
|
||||
|
||||
def set_drop_position(self, position: Real_Position):
|
||||
"""
|
||||
设置 FDropBag 位置，只设置当前点，不处理前后点。
|
||||
@ -179,6 +190,23 @@ class FeedLine:
|
||||
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
|
||||
break # 假设只有一个丢包点
|
||||
|
||||
def get_drop_path(self) -> list:
|
||||
"""获取动态扔包路径"""
|
||||
|
||||
if self.drop_manager is None:
|
||||
return []
|
||||
|
||||
path = []
|
||||
while True:
|
||||
pos_model = self.drop_manager.get_next_drop_position(
|
||||
self.id,
|
||||
self.current_index
|
||||
)
|
||||
if pos_model is None:
|
||||
break
|
||||
path.append(pos_model)
|
||||
return path
|
||||
|
||||
def get_position_list(self):
|
||||
index_start = -1
|
||||
for i in range(len(self.feed_positions)):
|
||||
@ -189,20 +217,46 @@ 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
|
||||
|
||||
# 开始插入动态扔包中间点
|
||||
|
||||
# 开始插入动态扔包点
|
||||
|
||||
# 开始插入复位中间点
|
||||
|
||||
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]
|
||||
self.feeding_to_end = self.feed_positions[index_take:]
|
||||
|
||||
for i in range(len(self.feeding_to_end)): #插入动态中间点
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
befor_position_model = PositionModel()
|
||||
befor_position_model.init_position(None)
|
||||
after_position_model = PositionModel()
|
||||
after_position_model.init_position(None)
|
||||
# 将总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)
|
||||
|
||||
# for i in range(len(self.feeding_to_end)): #插入动态中间点
|
||||
# if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
# befor_position_model = PositionModel()
|
||||
# befor_position_model.init_position(None)
|
||||
# after_position_model = PositionModel()
|
||||
# after_position_model.init_position(None)
|
||||
#
|
||||
# self.feeding_to_end.insert(i, befor_position_model)
|
||||
# self.feeding_to_end.insert(i+2, after_position_model)
|
||||
# break
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
self.feeding_to_end.insert(i, befor_position_model)
|
||||
self.feeding_to_end.insert(i+2, after_position_model)
|
||||
break
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
|
||||
@ -234,7 +288,6 @@ class Feeding(QObject):
|
||||
self.relay_controller = RelayController()
|
||||
self.sensor_thread = None
|
||||
self.detection_image = None
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
self.cRis_shake = CRisOrFall()
|
||||
@ -267,8 +320,7 @@ class Feeding(QObject):
|
||||
self.sensor2_thread = None
|
||||
|
||||
# --- 新增: 用于码垛模式的投料点索引 ---
|
||||
self.current_drop_index = 0
|
||||
self.drop_manager = DropPositionManager("CU/drop.ini")
|
||||
self.current_drop_index = 1
|
||||
pass
|
||||
|
||||
def close_feed(self):
|
||||
@ -276,10 +328,6 @@ class Feeding(QObject):
|
||||
self.detect_thread.join()
|
||||
self.detect.detection.release()
|
||||
|
||||
def init_detection_image(self):
|
||||
detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
self.update_detect_image.emit(detection_image)
|
||||
|
||||
def run_detect(self):
|
||||
while self.is_detected:
|
||||
self.detect.run()
|
||||
@ -289,7 +337,7 @@ class Feeding(QObject):
|
||||
self.catch.run()
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
self.detect.position_index = 0
|
||||
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
@ -388,7 +436,6 @@ class Feeding(QObject):
|
||||
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
self.next_position(self.is_reverse)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
|
||||
self.init_detection_image()
|
||||
return
|
||||
|
||||
if not Constant.Debug:
|
||||
@ -399,6 +446,7 @@ class Feeding(QObject):
|
||||
#self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
|
||||
'''real_position'''
|
||||
# 一直等待传感器2信号，永不超时
|
||||
# TODO:逻辑需改变，不能用while循环
|
||||
while True:
|
||||
sensors = self.relay_controller.get_all_device_status('sensors')
|
||||
sensor2_value = sensors.get(self.relay_controller.SENSOR2, False)
|
||||
@ -408,10 +456,11 @@ 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
|
||||
# self.feedStatus = FeedStatus.FTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take)
|
||||
@ -420,9 +469,11 @@ class Feeding(QObject):
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
return
|
||||
if not take_position.get_position().compare(real_position, is_action=True):
|
||||
self.log_signal.emit(logging.INFO, "🟡 机器人尚未到达抓料点位")
|
||||
self.log_signal.emit(logging.INFO, "机器人尚未到达抓料点位")
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
|
||||
self.log_signal.emit(logging.INFO, "机器人已到达抓料点位")
|
||||
|
||||
|
||||
# 执行抓取动作
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
#if self.catch.catch_status == CatchStatus.CNone:
|
||||
@ -435,17 +486,14 @@ class Feeding(QObject):
|
||||
#self.catch.catch_status = CatchStatus.CNone
|
||||
# 移动到下一个抓取点
|
||||
# 更新丢包点: 如果需要根据放置情况调整下次抓取
|
||||
next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1)
|
||||
print("next_drop_pos--------",next_drop_pos)
|
||||
if next_drop_pos:
|
||||
self.feedConfig.feedLine.set_drop_position(next_drop_pos)
|
||||
self.log_signal.emit(logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}")
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, "获取放置点失败")
|
||||
return
|
||||
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
|
||||
|
||||
self.relay_controller.open(clamp=True)
|
||||
self.next_position()
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
|
||||
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
|
||||
|
||||
|
||||
#return
|
||||
#else:
|
||||
#self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
@ -457,11 +505,13 @@ class Feeding(QObject):
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
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:
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
@ -524,6 +574,12 @@ class Feeding(QObject):
|
||||
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
|
||||
# 调用 next_Feed()。
|
||||
self.next_position()
|
||||
elif self.feedStatus == FeedStatus.FDropReset:
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == None:
|
||||
print(self.feedStatus)
|
||||
pass
|
||||
|
||||
def run_reset(self):
|
||||
real_position = Real_Position()
|
||||
@ -741,11 +797,14 @@ class Feeding(QObject):
|
||||
self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position())
|
||||
elif start_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
|
||||
self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=start_pos.get_position(), speed=self.robotClient.reset_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
# if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),self.robotClient.max_angle_interval):
|
||||
# self.feedStatus = None
|
||||
# self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
# else:
|
||||
# self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
|
||||
pass
|
||||
@ -759,13 +818,17 @@ class Feeding(QObject):
|
||||
self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
elif take_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
# if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
|
||||
# self.robotClient.max_angle_interval):
|
||||
# self.feedStatus = None
|
||||
# self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
# else:
|
||||
# self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
# move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
@ -779,13 +842,20 @@ class Feeding(QObject):
|
||||
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
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:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
# 暂时不考虑世界坐标到关节坐标的转换，强行不判断接近
|
||||
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)
|
||||
|
||||
# 注释掉 by chuyiwen
|
||||
# if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
|
||||
# self.robotClient.max_angle_interval):
|
||||
# self.feedStatus = FeedStatus.FNone
|
||||
# self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
# else:
|
||||
# self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
# move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
# if not reverse and self.feedStatus == FeedStatus.FShake:
|
||||
@ -807,6 +877,8 @@ class Feeding(QObject):
|
||||
return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
|
||||
|
||||
|
||||
def next_position(self,reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
self.next_start(reverse)
|
||||
|
||||
Reference in New Issue
Block a user