forked from huangxin/ailai
354 lines
16 KiB
Python
354 lines
16 KiB
Python
|
import time
|
|||
|
|
import threading
|
|||
|
|
from enum import IntEnum
|
|||
|
|
|
|||
|
|
# --- 1. 简化模拟必要的 Enum 和类 ---
|
|||
|
|
|
|||
|
|
class FeedStatus(IntEnum):
|
|||
|
|
FNone = 0
|
|||
|
|
FStart = 1
|
|||
|
|
FCheck = 2
|
|||
|
|
FMid = 3
|
|||
|
|
FPhoto = 4
|
|||
|
|
FTake = 5
|
|||
|
|
FDropBag = 9 # 简化，只保留关键状态
|
|||
|
|
|
|||
|
|
class CatchStatus:
|
|||
|
|
CNone = 0
|
|||
|
|
CTake = 1
|
|||
|
|
COk = 2
|
|||
|
|
CDrop = 3
|
|||
|
|
|
|||
|
|
class Real_Position:
|
|||
|
|
"""简化的位置表示"""
|
|||
|
|
def __init__(self, name=""):
|
|||
|
|
self.name = name
|
|||
|
|
|
|||
|
|
def compare(self, other, is_action=False):
|
|||
|
|
# 简化比较，只比较名字
|
|||
|
|
return self.name == other.name
|
|||
|
|
|
|||
|
|
def __str__(self):
|
|||
|
|
return self.name
|
|||
|
|
|
|||
|
|
class PositionModel:
|
|||
|
|
"""简化的位置模型"""
|
|||
|
|
def __init__(self, status, position):
|
|||
|
|
self.status = status
|
|||
|
|
self.position = position
|
|||
|
|
|
|||
|
|
def get_position(self):
|
|||
|
|
return self.position
|
|||
|
|
|
|||
|
|
class FeedLine:
|
|||
|
|
"""简化 FeedLine，只包含关键点"""
|
|||
|
|
def __init__(self, name, positions: list): # positions: list of PositionModel
|
|||
|
|
self.name = name
|
|||
|
|
# 假设 positions 按顺序排列，且包含 FTake 和 FDropBag 点
|
|||
|
|
self.positions = positions
|
|||
|
|
self.take_position_model = None
|
|||
|
|
self.drop_position_model = None
|
|||
|
|
self._find_key_positions()
|
|||
|
|
|
|||
|
|
def _find_key_positions(self):
|
|||
|
|
for pm in self.positions:
|
|||
|
|
if pm.status == FeedStatus.FTake:
|
|||
|
|
self.take_position_model = pm
|
|||
|
|
elif pm.status == FeedStatus.FDropBag:
|
|||
|
|
self.drop_position_model = pm
|
|||
|
|
|
|||
|
|
def get_take_position(self):
|
|||
|
|
return self.take_position_model
|
|||
|
|
|
|||
|
|
# --- 2. 简化模拟 Catch 模块 ---
|
|||
|
|
class SimulatedCatch:
|
|||
|
|
def __init__(self, log_func):
|
|||
|
|
self.catch_status = CatchStatus.CNone
|
|||
|
|
self.log_signal = log_func # 用于打印日志的函数
|
|||
|
|
|
|||
|
|
def run(self):
|
|||
|
|
"""模拟抓取动作"""
|
|||
|
|
if self.catch_status == CatchStatus.CTake:
|
|||
|
|
self.log_signal(logging.INFO, "🤖 Sim Catch: 开始执行抓取动作...")
|
|||
|
|
time.sleep(1) # 模拟抓取耗时
|
|||
|
|
self.catch_status = CatchStatus.COk
|
|||
|
|
self.log_signal(logging.INFO, "🎉 Sim Catch: 抓取完成!")
|
|||
|
|
elif self.catch_status == CatchStatus.CDrop:
|
|||
|
|
self.log_signal(logging.INFO, "📦 Sim Catch: 开始执行投料动作...")
|
|||
|
|
time.sleep(0.5) # 模拟投料耗时
|
|||
|
|
self.catch_status = CatchStatus.COk
|
|||
|
|
self.log_signal(logging.INFO, "🎊 Sim Catch: 投料完成!")
|
|||
|
|
|
|||
|
|
# --- 3. 简化 FeedingConfig ---
|
|||
|
|
class FeedingConfig:
|
|||
|
|
def __init__(self, num: int, feedLine: FeedLine, drop_positions: list):
|
|||
|
|
self.num = num
|
|||
|
|
self.feedLine = feedLine
|
|||
|
|
self.drop_positions = drop_positions # list of Real_Position
|
|||
|
|
|
|||
|
|
# --- 4. 简化 RobotClient 状态 ---
|
|||
|
|
class SimulatedRobotClient:
|
|||
|
|
def __init__(self, initial_position_name):
|
|||
|
|
self.status_model = type('obj', (object,), {'world_0': 0, 'world_1': 0, 'world_2': 0, 'world_3': 0, 'world_4': 0, 'world_5': 0})()
|
|||
|
|
self.origin_position = Real_Position("Origin")
|
|||
|
|
self.feed_speed = 10
|
|||
|
|
# 当前模拟位置
|
|||
|
|
self.current_position = Real_Position(initial_position_name)
|
|||
|
|
|
|||
|
|
def getRealPosition(self):
|
|||
|
|
return self.current_position
|
|||
|
|
|
|||
|
|
# --- 5. 模拟传感器 ---
|
|||
|
|
class SimulatedSensor:
|
|||
|
|
def __init__(self, name, initial_state=False):
|
|||
|
|
self.name = name
|
|||
|
|
self.state = initial_state
|
|||
|
|
self.thread = None
|
|||
|
|
self._running = False
|
|||
|
|
|
|||
|
|
def start_simulation(self, sequence):
|
|||
|
|
"""根据一个序列 (时间, 状态) 来模拟传感器状态变化"""
|
|||
|
|
self._running = True
|
|||
|
|
def _run():
|
|||
|
|
start_time = time.time()
|
|||
|
|
seq_index = 0
|
|||
|
|
while self._running and seq_index < len(sequence):
|
|||
|
|
elapsed = time.time() - start_time
|
|||
|
|
if elapsed >= sequence[seq_index][0]: # sequence[seq_index][0] 是触发时间
|
|||
|
|
self.state = sequence[seq_index][1] # sequence[seq_index][1] 是触发状态
|
|||
|
|
print(f" 📡 [{self.name}] 模拟状态变为: {self.state}")
|
|||
|
|
seq_index += 1
|
|||
|
|
time.sleep(0.1) # 检查间隔
|
|||
|
|
self.thread = threading.Thread(target=_run, daemon=True)
|
|||
|
|
self.thread.start()
|
|||
|
|
|
|||
|
|
def stop_simulation(self):
|
|||
|
|
self._running = False
|
|||
|
|
if self.thread:
|
|||
|
|
self.thread.join()
|
|||
|
|
|
|||
|
|
# --- 6. 简化版 Feeding 类 (核心逻辑) ---
|
|||
|
|
import logging
|
|||
|
|
|
|||
|
|
class SimulatedFeeding:
|
|||
|
|
def __init__(self, robotClient: SimulatedRobotClient, sensor2: SimulatedSensor):
|
|||
|
|
self.feedConfig = None
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
self.robotClient = robotClient
|
|||
|
|
self.sensor2 = sensor2 # 使用模拟传感器
|
|||
|
|
self.pause = False
|
|||
|
|
self.is_reverse = False
|
|||
|
|
self.catch = SimulatedCatch(self._simple_log) # 使用模拟 Catch
|
|||
|
|
# --- 码垛模式新增 ---
|
|||
|
|
self.current_drop_index = 0
|
|||
|
|
|
|||
|
|
# 简化的日志信号
|
|||
|
|
# self.log_signal = self._simple_log
|
|||
|
|
self.log_signal = lambda level, message: self._simple_log(level, message)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _simple_log(self, level, message):
|
|||
|
|
level_str = {logging.INFO: "INFO", logging.ERROR: "ERROR", logging.WARNING: "WARN"}.get(level, "LOG")
|
|||
|
|
print(f"[{level_str}] {message}")
|
|||
|
|
|
|||
|
|
def set_config(self, config: FeedingConfig):
|
|||
|
|
self.feedConfig = config
|
|||
|
|
|
|||
|
|
def start(self):
|
|||
|
|
if self.feedConfig:
|
|||
|
|
self.feedStatus = FeedStatus.FStart
|
|||
|
|
self.log_signal(logging.INFO, "🚀 启动仿真投料流程")
|
|||
|
|
# self.log_signal(logging.INFO, "🚀 启动仿真投料流程")
|
|||
|
|
|
|||
|
|
def run(self):
|
|||
|
|
"""仿真运行一次主循环"""
|
|||
|
|
if self.feedConfig is None or self.feedStatus == FeedStatus.FNone or self.pause:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 获取当前模拟位置
|
|||
|
|
real_position = self.robotClient.current_position
|
|||
|
|
|
|||
|
|
# --- 简化版 FStart ---
|
|||
|
|
if self.feedStatus == FeedStatus.FStart:
|
|||
|
|
# self.log_signal(logging.INFO, "🟢 FStart: 初始化流程")
|
|||
|
|
self.log_signal(logging.INFO, "🟢 FStart: 初始化流程")
|
|||
|
|
self.current_drop_index = 0 # 重置投料点索引
|
|||
|
|
# 模拟移动到抓取点 (直接设置位置)
|
|||
|
|
take_pos_model = self.feedConfig.feedLine.get_take_position()
|
|||
|
|
if take_pos_model:
|
|||
|
|
self.robotClient.current_position = take_pos_model.get_position()
|
|||
|
|
self.log_signal(logging.INFO, f"🚚 FStart: 移动到抓取点 {self.robotClient.current_position}")
|
|||
|
|
# self.log_signal(logging.INFO, f"🚚 FStart: 移动到抓取点 {self.robotClient.current_position}")
|
|||
|
|
self.feedStatus = FeedStatus.FTake
|
|||
|
|
else:
|
|||
|
|
# self.log_signal(logging.ERROR, "❌ FStart: 未找到抓取点!")
|
|||
|
|
self.log_signal(logging.ERROR, "❌ FStart: 未找到抓取点!")
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
return # 等待下次循环
|
|||
|
|
|
|||
|
|
# --- 简化版 FTake ---
|
|||
|
|
elif self.feedStatus == FeedStatus.FTake:
|
|||
|
|
# self.log_signal(logging.INFO, "✋ FTake: 检查是否到达抓取点并等待传感器信号")
|
|||
|
|
self.log_signal(logging.INFO, "✋ FTake: 检查是否到达抓取点并等待传感器信号")
|
|||
|
|
take_position_model = self.feedConfig.feedLine.get_take_position()
|
|||
|
|
if not take_position_model or not take_position_model.get_position():
|
|||
|
|
self.log_signal(logging.ERROR, "❌ FTake: 抓取点配置错误!")
|
|||
|
|
# self.log_signal(logging.ERROR, "❌ FTake: 抓取点配置错误!")
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
if not take_position_model.get_position().compare(real_position):
|
|||
|
|
self.log_signal(logging.WARNING, f"⚠️ FTake: 机器人未在抓取点 ({take_position_model.get_position()}), 当前在 {real_position}")
|
|||
|
|
# self.log_signal(logging.WARNING, f"⚠️ FTake: 机器人未在抓取点 ({take_position_model.get_position()}), 当前在 {real_position}")
|
|||
|
|
# 在仿真中，我们假设它会移动到位，所以直接设置
|
|||
|
|
self.robotClient.current_position = take_position_model.get_position()
|
|||
|
|
self.log_signal(logging.INFO, f"🔧 强制移动到抓取点 {self.robotClient.current_position}")
|
|||
|
|
# self.log_signal(logging.INFO, f"🔧 强制移动到抓取点 {self.robotClient.current_position}")
|
|||
|
|
|
|||
|
|
# 模拟等待传感器2信号
|
|||
|
|
# 在实际仿真中，我们可以通过预设序列或用户输入来控制
|
|||
|
|
# 这里用一个简单的循环等待 (实际应用中应避免阻塞)
|
|||
|
|
wait_start = time.time()
|
|||
|
|
timeout = 10 # 10秒超时
|
|||
|
|
while not self.sensor2.state:
|
|||
|
|
if time.time() - wait_start > timeout:
|
|||
|
|
# self.log_signal(logging.ERROR, "⏰ FTake: 等待传感器2超时!")
|
|||
|
|
self.log_signal(logging.ERROR, "⏰ FTake: 等待传感器2超时!")
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
return
|
|||
|
|
self.log_signal(logging.INFO, "⏳ FTake: 等待传感器2信号...")
|
|||
|
|
time.sleep(1) # 每秒检查一次
|
|||
|
|
|
|||
|
|
self.log_signal(logging.INFO, "✅ FTake: 传感器2信号收到，开始抓取")
|
|||
|
|
# 执行抓取动作
|
|||
|
|
if self.catch.catch_status == CatchStatus.CNone:
|
|||
|
|
self.catch.catch_status = CatchStatus.CTake
|
|||
|
|
return # 等待下一周期 Catch 完成
|
|||
|
|
if self.catch.catch_status == CatchStatus.CTake:
|
|||
|
|
return # 等待 Catch 模块处理
|
|||
|
|
if self.catch.catch_status == CatchStatus.COk:
|
|||
|
|
self.catch.catch_status = CatchStatus.CNone
|
|||
|
|
self.log_signal(logging.INFO, "👏 FTake: 抓取成功，准备投料")
|
|||
|
|
# 移动到第一个投料点
|
|||
|
|
if self.feedConfig.drop_positions:
|
|||
|
|
target_drop_pos = self.feedConfig.drop_positions[self.current_drop_index]
|
|||
|
|
self.robotClient.current_position = target_drop_pos
|
|||
|
|
self.log_signal(logging.INFO, f"🚚 FTake: 移动到投料点 {self.current_drop_index} ({target_drop_pos})")
|
|||
|
|
self.feedStatus = FeedStatus.FDropBag
|
|||
|
|
else:
|
|||
|
|
self.log_signal(logging.ERROR, "❌ FTake: 投料点列表为空!")
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# --- 简化版 FDropBag (码垛核心) ---
|
|||
|
|
elif self.feedStatus == FeedStatus.FDropBag:
|
|||
|
|
if not self.feedConfig.drop_positions:
|
|||
|
|
self.log_signal(logging.ERROR, "❌ FDropBag: 投料点列表为空!")
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
target_drop_position = self.feedConfig.drop_positions[self.current_drop_index]
|
|||
|
|
self.log_signal(logging.INFO, f"📦 FDropBag: 当前目标投料点索引: {self.current_drop_index} ({target_drop_position})")
|
|||
|
|
|
|||
|
|
# 检查是否到达目标投料点 (仿真中直接比较)
|
|||
|
|
if target_drop_position.compare(real_position):
|
|||
|
|
self.log_signal(logging.INFO, f"🎯 FDropBag: 到达投料点 {self.current_drop_index}")
|
|||
|
|
|
|||
|
|
# 执行投料动作
|
|||
|
|
if self.catch.catch_status == CatchStatus.CNone:
|
|||
|
|
self.catch.catch_status = CatchStatus.CDrop
|
|||
|
|
self.log_signal(logging.INFO, "🪂 FDropBag: 开始执行投料动作...")
|
|||
|
|
return # 等待下一周期检查 COk
|
|||
|
|
if self.catch.catch_status == CatchStatus.CDrop:
|
|||
|
|
self.log_signal(logging.INFO, "🪂 FDropBag: 投料动作进行中...")
|
|||
|
|
return # 等待 Catch 模块完成
|
|||
|
|
if self.catch.catch_status == CatchStatus.COk:
|
|||
|
|
self.catch.catch_status = CatchStatus.CNone
|
|||
|
|
self.log_signal(logging.INFO, f"🎉 FDropBag: 在投料点 {self.current_drop_index} 投料完成")
|
|||
|
|
|
|||
|
|
# 更新投料计数
|
|||
|
|
self.feedConfig.num -= 1
|
|||
|
|
self.log_signal(logging.INFO, f'🔢 剩余投料次数: {self.feedConfig.num}')
|
|||
|
|
|
|||
|
|
# 检查是否所有投料任务完成
|
|||
|
|
if self.feedConfig.num > 0:
|
|||
|
|
# 更新投料点索引 (循环)
|
|||
|
|
self.current_drop_index = (self.current_drop_index + 1) % len(self.feedConfig.drop_positions)
|
|||
|
|
self.log_signal(logging.INFO, f"🔄 更新投料点索引为: {self.current_drop_index}")
|
|||
|
|
|
|||
|
|
# 移动到固定的抓取点 (仿真中直接设置位置)
|
|||
|
|
take_position_model = self.feedConfig.feedLine.get_take_position()
|
|||
|
|
if take_position_model:
|
|||
|
|
self.log_signal(logging.INFO, f"🔄 移动到固定抓取点...")
|
|||
|
|
self.robotClient.current_position = take_position_model.get_position()
|
|||
|
|
self.log_signal(logging.INFO, f"📍 已移动到抓取点 {self.robotClient.current_position}")
|
|||
|
|
self.feedStatus = FeedStatus.FTake # 设置状态为 FTake
|
|||
|
|
else:
|
|||
|
|
self.log_signal(logging.ERROR, "❌ 无法获取抓取点位置！")
|
|||
|
|
self.feedStatus = FeedStatus.FNone
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
# 所有投料完成
|
|||
|
|
self.log_signal(logging.INFO, "🎊 所有投料任务完成")
|
|||
|
|
self.feedStatus = FeedStatus.FNone # 结束流程
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
# 如果还没到达目标投料点 (仿真中直接移动)
|
|||
|
|
self.log_signal(logging.INFO, f"🚚 正在移动到投料点 {self.current_drop_index} ({target_drop_position})")
|
|||
|
|
self.robotClient.current_position = target_drop_position
|
|||
|
|
self.log_signal(logging.INFO, f"📍 已移动到投料点 {self.current_drop_index} ({self.robotClient.current_position})")
|
|||
|
|
|
|||
|
|
# --- 7. 主程序入口 ---
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
# --- 配置仿真环境 ---
|
|||
|
|
logging.basicConfig(level=logging.INFO) # 设置日志级别
|
|||
|
|
|
|||
|
|
# 1. 定义位置点
|
|||
|
|
origin_pos = Real_Position("Origin")
|
|||
|
|
take_pos = Real_Position("Fixed_Take_Point")
|
|||
|
|
drop_pos_1 = Real_Position("Drop_Point_1")
|
|||
|
|
drop_pos_2 = Real_Position("Drop_Point_2")
|
|||
|
|
drop_pos_3 = Real_Position("Drop_Point_3")
|
|||
|
|
|
|||
|
|
# 2. 创建 FeedLine (简化)
|
|||
|
|
feedline_positions = [
|
|||
|
|
PositionModel(FeedStatus.FTake, take_pos),
|
|||
|
|
PositionModel(FeedStatus.FDropBag, Real_Position("Placeholder_Drop_Pos")) # 占位，实际由 drop_positions 列表决定
|
|||
|
|
]
|
|||
|
|
feed_line = FeedLine("SimFeedLine", feedline_positions)
|
|||
|
|
|
|||
|
|
# 3. 定义投料点列表 (码垛点)
|
|||
|
|
drop_points = [drop_pos_1, drop_pos_2, drop_pos_3]
|
|||
|
|
|
|||
|
|
# 4. 创建 FeedingConfig
|
|||
|
|
total_bags = 5 # 总共要投的料包数
|
|||
|
|
feeding_config = FeedingConfig(num=total_bags, feedLine=feed_line, drop_positions=drop_points)
|
|||
|
|
|
|||
|
|
# 5. 创建模拟机器人和传感器
|
|||
|
|
robot_client = SimulatedRobotClient("Origin")
|
|||
|
|
sensor2 = SimulatedSensor("Sensor2", initial_state=False) # 初始无信号
|
|||
|
|
|
|||
|
|
# 6. 创建并配置仿真 Feeding
|
|||
|
|
sim_feeding = SimulatedFeeding(robot_client, sensor2)
|
|||
|
|
sim_feeding.set_config(feeding_config)
|
|||
|
|
|
|||
|
|
# 7. (可选) 预设传感器2信号序列 [(时间(秒), 状态), ...]
|
|||
|
|
# 例如：在第2秒时变为True，第8秒时变为False，第15秒时再变为True...
|
|||
|
|
sensor2_sequence = [(2, True), (8, False), (15, True), (22, False), (30, True)]
|
|||
|
|
sensor2.start_simulation(sensor2_sequence)
|
|||
|
|
|
|||
|
|
# 8. 启动流程
|
|||
|
|
sim_feeding.start()
|
|||
|
|
|
|||
|
|
# 9. 运行仿真主循环
|
|||
|
|
try:
|
|||
|
|
while sim_feeding.feedStatus != FeedStatus.FNone:
|
|||
|
|
sim_feeding.run()
|
|||
|
|
sim_feeding.catch.run() # 触发模拟抓取/投料逻辑
|
|||
|
|
time.sleep(0.5) # 控制仿真循环速度
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n🛑 仿真被用户中断")
|
|||
|
|
finally:
|
|||
|
|
sensor2.stop_simulation()
|
|||
|
|
print("🏁 仿真结束")
|