from ast import mod import configparser import os from typing import Optional from Model.Position import Real_Position from Model.FeedModel import PositionModel class DropPositionManager: def __init__(self, config_path="CU/drop.ini"): self.config_path = config_path self.config = configparser.ConfigParser() self._load_config() # --- 本地缓存 --- self._current_lineid: Optional[int] = None self._current_point_id: Optional[int] = None self._current_path: list = [] # 当前路径点列表 self._current_index: int = 0 # 当前路径中的索引 def _load_config(self): """加载配置文件""" if not os.path.exists(self.config_path): raise FileNotFoundError(f"配置文件不存在: {self.config_path}") self.config.read(self.config_path, encoding='utf-8') def get_next_drop_position(self, lineid: int, point: int) -> Optional[PositionModel]: """ 获取指定 lineid 和 point 的下一个路径点。 :param lineid: 路径组 ID（对应 DropLineX） :param point: 投料点 ID（对应 DropPoints{point}） :return: 下一个路径点，若无则返回 None """ # 如果 lineid 或 point 改变，重新加载路径 if self._current_lineid != lineid or self._current_point_id != point: self._load_point_path(lineid, point) self._current_lineid = lineid self._current_point_id = point self._current_index = 0 # 返回当前索引的点 if self._current_index < len(self._current_path): next_point = self._current_path[self._current_index] self._current_index += 1 pos = next_point.get_position() print(f"🎯 返回点: status={next_point.status}, linetype={next_point.lineType}, " f"position=({pos.X:.3f}, {pos.Y:.3f}, {pos.Z:.3f})") return next_point # 路径结束 print("✅ 当前点集合路径已结束") return None def _load_point_path(self, lineid: int, point_id: int): """加载指定 lineid 和 point_id 的完整路径 (点集合dropmidpoint、droppoint、resetpoint)""" self._current_path = [] # 检查是否存在 DropPoints{point_id} drop_point_sec = f"DropPoints{point_id}" if not self.config.has_section(drop_point_sec): print(f"❌ 配置错误：不存在 {drop_point_sec}") return if self.config.getint(drop_point_sec, "lineid") != lineid: print(f"❌ {drop_point_sec} 不属于 lineid={lineid}") return try: drop_pos = self._read_position_from_section(drop_point_sec) except Exception as e: print(f"❌ 读取 {drop_point_sec} 失败: {e}") return # 1. 加载 DropMidPoint{point_id}-*（按 level 升序） mid_points = [] for sec in self.config.sections(): if sec.startswith(f"DropMidPoint{point_id}-") and self.config.getint(sec, "lineid") == lineid: try: level = int(sec.split('-')[1]) pos = self._read_position_from_section(sec) mid_points.append((level, pos)) except Exception as e: print(f"❌ 解析 {sec} 失败: {e}") mid_points.sort(key=lambda x: x[0]) # 2. 加载 ResetPoint{point_id}-*（按 level 升序） reset_points = [] for sec in self.config.sections(): if sec.startswith(f"ResetPoint{point_id}-") and self.config.getint(sec, "lineid") == lineid: try: level = int(sec.split('-')[1]) pos = self._read_position_from_section(sec) reset_points.append((level, pos)) except Exception as e: print(f"❌ 解析 {sec} 失败: {e}") reset_points.sort(key=lambda x: x[0]) # 3. 组装路径 # a. DropMidPoint for _, pos in mid_points: # model = PositionModel() # model.init_position(pos) # model.status = 7 # FMid # model.lineType = 4 # WORLD self._current_path.append(pos) # b. DropPoints # main_model = PositionModel() # main_model.init_position(drop_pos) # main_model.status = 9 # FDropBag # main_model.lineType = 4 self._current_path.append(drop_pos) # c. ResetPoint for _, pos in reset_points: # model = PositionModel() # model.init_position(pos) # model.status = 10 # FReverse # model.lineType = 4 self._current_path.append(pos) print(f"✅ 已加载 DropLine{lineid} 中 DropPoints{point_id} 的路径，共 {len(self._current_path)} 个点") def _read_position_from_section(self, section: str) -> PositionModel: """从配置文件的 section 中读取位置信息""" model = PositionModel() pos = Real_Position() pos.X = self.config.getfloat(section, "x") pos.Y = self.config.getfloat(section, "y") pos.Z = self.config.getfloat(section, "z") pos.U = self.config.getfloat(section, "u") pos.V = self.config.getfloat(section, "v") pos.W = self.config.getfloat(section, "w") model.init_position(pos) model.lineType=self.config.getint(section, "linetype") model.status=self.config.getint(section, "status") return model def _get_point_debug_info(manager, pos, model): config = manager.config for sec in config.sections(): if sec.startswith("DropPoints"): try: x, y, z = config.getfloat(sec, "x"), config.getfloat(sec, "y"), config.getfloat(sec, "z") if abs(x - pos.X) < 0.001 and abs(y - pos.Y) < 0.001 and abs(z - pos.Z) < 0.001: point_id = config.getint(sec, "id") return f"📌 DropPoints{point_id} | id={point_id}" except: pass elif sec.startswith("DropMidPoint"): try: parts = sec.split('-') if len(parts) != 2: continue point_id = int(''.join(filter(str.isdigit, parts[0]))) level = int(parts[1]) x, y, z = config.getfloat(sec, "x"), config.getfloat(sec, "y"), config.getfloat(sec, "z") if abs(x - pos.X) < 0.001 and abs(y - pos.Y) < 0.001 and abs(z - pos.Z) < 0.001: return f"📍 DropMidPoint{point_id}-{level} | id={point_id}, level={level}" except: pass elif sec.startswith("ResetPoint"): try: parts = sec.split('-') if len(parts) != 2: continue point_id = int(''.join(filter(str.isdigit, parts[0]))) level = int(parts[1]) x, y, z = config.getfloat(sec, "x"), config.getfloat(sec, "y"), config.getfloat(sec, "z") if abs(x - pos.X) < 0.001 and abs(y - pos.Y) < 0.001 and abs(z - pos.Z) < 0.001: return f"🔙 ResetPoint{point_id}-{level} | id={point_id}, level={level}" except: pass return "❓ 未知点位" # 测试 if __name__ == "__main__": # manager = DropPositionManager("drop.ini") manager = DropPositionManager() lineid = 1 print(f"\n🔁 测试：通过 point 参数切换路径集合\n") # 先走 point=1 print("=" * 60) print("📦 开始执行 point=1 的路径") print("=" * 60) for i in range(10): pos_model = manager.get_next_drop_position(lineid=lineid, point=1) if pos_model is None: print("🔚 point=1 路径结束") break pos = pos_model.get_position() info = _get_point_debug_info(manager, pos, pos_model) print(f" 🚀 {info}") if pos_model.status == 9: print(" 💥 执行【扔包】操作！") # 再走 point=2 print("\n" + "=" * 60) print("📦 开始执行 point=2 的路径") print("=" * 60) for i in range(10): pos_model = manager.get_next_drop_position(lineid=lineid, point=2) if pos_model is None: print("🔚 point=2 路径结束") break pos = pos_model.get_position() info = _get_point_debug_info(manager, pos, pos_model) print(f" 🚀 {info}") if pos_model.status == 9: print(" 💥 执行【扔包】操作！") # 再切回 point=1 print("\n" + "=" * 60) print("🔄 切换回 point=1，从头开始") print("=" * 60) for i in range(3): pos_model = manager.get_next_drop_position(lineid=lineid, point=1) if pos_model is None: break pos = pos_model.get_position() info = _get_point_debug_info(manager, pos, pos_model) print(f" 🚀 {info}")