import json import logging import os import random import time import uuid import string from typing import Dict, List, Tuple, Optional, Any from datetime import datetime def setup_logging(log_level: str = "INFO", log_file: Optional[str] = None) -> None: """设置日志配置""" log_format = '%(asctime)s - %(name)s - %(levelname)s - %(message)s' formatter = logging.Formatter(log_format) root_logger = logging.getLogger() root_logger.setLevel(getattr(logging, log_level.upper())) for handler in root_logger.handlers[:]: root_logger.removeHandler(handler) console_handler = logging.StreamHandler() console_handler.setFormatter(formatter) root_logger.addHandler(console_handler) if log_file: file_handler = logging.FileHandler(log_file, encoding='utf-8') file_handler.setFormatter(formatter) root_logger.addHandler(file_handler) def load_path_mapping(mapping_file: str = "path_mapping.json") -> Dict[str, Dict[str, int]]: """加载路径映射文件""" logger = logging.getLogger(__name__) try: with open(mapping_file, 'r', encoding='utf-8') as f: data = json.load(f) path_mapping = {} if "path_id_to_coordinates" in data: for path_id, coordinates in data["path_id_to_coordinates"].items(): if coordinates and len(coordinates) > 0: coord = coordinates[0] path_mapping[path_id] = { "x": coord["x"], "y": coord["y"] } logger.info(f"成功加载路径映射，共 {len(path_mapping)} 个路径点") return path_mapping except FileNotFoundError: logger.error(f"路径映射文件不存在: {mapping_file}") return {} except json.JSONDecodeError as e: logger.error(f"路径映射文件格式错误: {e}") return {} except Exception as e: logger.error(f"加载路径映射文件失败: {e}") return {} def get_coordinate_from_path_id(path_id: str, path_mapping: Dict[str, Dict[str, int]]) -> Optional[Tuple[int, int]]: """ 根据路径点ID获取坐标 Args: path_id: 路径点ID（支持带8位带零的格式，e.g.'00000206'） path_mapping: 路径映射字典 Returns: Optional[Tuple[int, int]]: 坐标(x, y)，如果找不到则返回None """ logger = logging.getLogger(__name__) if path_id in path_mapping: coord = path_mapping[path_id] logger.debug(f"路径ID {path_id} 匹配成功，坐标: ({coord['x']}, {coord['y']})") return (coord["x"], coord["y"]) # 如果匹配失败，尝试去掉零后匹配 try: normalized_path_id = str(int(path_id)) if normalized_path_id in path_mapping: coord = path_mapping[normalized_path_id] logger.debug(f"路径ID {path_id} 规范化为 {normalized_path_id} 后匹配成功，坐标: ({coord['x']}, {coord['y']})") return (coord["x"], coord["y"]) else: logger.warning(f"规范化后的路径ID {normalized_path_id}（原始: {path_id}）在路径映射中未找到") except (ValueError, TypeError): logger.warning(f"路径ID {path_id} 不是有效的数字格式") logger.warning(f"无法找到路径ID {path_id} 对应的坐标") return None def get_path_id_from_coordinate(x: int, y: int, path_mapping: Dict[str, Dict[str, int]]) -> Optional[str]: """根据坐标获取路径点ID""" for path_id, coord in path_mapping.items(): if coord["x"] == x and coord["y"] == y: return path_id return None def calculate_distance(pos1: Tuple[int, int], pos2: Tuple[int, int]) -> float: """计算两点之间的欧氏距离""" return ((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2) ** 0.5 def calculate_manhattan_distance(pos1: Tuple[int, int], pos2: Tuple[int, int]) -> int: """计算两点之间的曼哈顿距离""" return abs(pos1[0] - pos2[0]) + abs(pos1[1] - pos2[1]) def generate_random_agv_id() -> str: """生成随机AGV ID""" return f"AGV_{random.randint(10001, 99999)}" def generate_random_task_id() -> str: """生成随机任务ID""" timestamp = int(time.time() * 1000) # 毫秒时间戳 random_suffix = random.randint(100, 999) return f"TASK_{timestamp}_{random_suffix}" def get_random_path_ids(path_mapping: Dict[str, Dict[str, int]], count: int = 2) -> List[str]: """随机获取路径点ID""" if not path_mapping: return [] path_ids = list(path_mapping.keys()) if len(path_ids) < count: return path_ids return random.sample(path_ids, count) def format_timestamp(timestamp: Optional[float] = None) -> str: """格式化时间戳""" if timestamp is None: timestamp = time.time() return datetime.fromtimestamp(timestamp).strftime("%Y-%m-%d %H:%M:%S") def normalize_path_id(path_id: str) -> str: """标准化路径点ID格式""" if not path_id: return path_id try: normalized_path_id = str(int(path_id)) return normalized_path_id except (ValueError, TypeError): return path_id def validate_path_id(path_id: str) -> bool: """验证路径点ID格式""" try: # 路径点ID应该是数字字符串，在1-1696范围内 path_num = int(path_id) return 1 <= path_num <= 1696 except (ValueError, TypeError): return False def validate_agv_id(agv_id: str) -> bool: """验证AGV ID格式""" if not agv_id or not isinstance(agv_id, str): return False # AGV ID不能为空且长度合理 return len(agv_id.strip()) > 0 and len(agv_id) <= 50 def save_json_file(data: Any, file_path: str) -> bool: """保存数据到JSON""" logger = logging.getLogger(__name__) try: with open(file_path, 'w', encoding='utf-8') as f: json.dump(data, f, ensure_ascii=False, indent=2) logger.info(f"数据已保存到文件: {file_path}") return True except Exception as e: logger.error(f"保存文件失败: {file_path} - {e}") return False def load_json_file(file_path: str) -> Optional[Any]: """从JSON文件加载数据""" logger = logging.getLogger(__name__) try: with open(file_path, 'r', encoding='utf-8') as f: data = json.load(f) logger.info(f"数据已从文件加载: {file_path}") return data except FileNotFoundError: logger.warning(f"文件不存在: {file_path}") return None except json.JSONDecodeError as e: logger.error(f"JSON格式错误: {file_path} - {e}") return None except Exception as e: logger.error(f"加载文件失败: {file_path} - {e}") return None def ensure_directory_exists(directory: str) -> bool: """确保目录存在，如果不存在则创建""" try: os.makedirs(directory, exist_ok=True) return True except Exception as e: logging.getLogger(__name__).error(f"创建目录失败: {directory} - {e}") return False def generate_segment_id(agv_id: str, task_id: Optional[str] = None, target_position: Optional[str] = None, action_type: str = "2") -> str: """ 生成导航段ID，用于去重标识 基于AGV ID、任务ID、目标位置和动作类型生成固定的11位ID Args: agv_id: AGV ID task_id: 任务ID target_position: 目标位置 action_type: 动作类型 Returns: str: 11位导航段ID """ import hashlib # 构建用于哈希的字符串 if task_id: # 任务类型：使用agv_id + task_id hash_input = f"{agv_id}_{task_id}_{action_type}" else: # 非任务类型：使用agv_id + target_position + action_type target = target_position or "unknown" hash_input = f"{agv_id}_{target}_{action_type}" # 使用MD5哈希生成固定值 hash_object = hashlib.md5(hash_input.encode()) hash_hex = hash_object.hexdigest() # 将16进制转换为数字并截取11位 hash_int = int(hash_hex[:8], 16) seg_id = str(hash_int)[-11:].zfill(11) return seg_id def generate_navigation_code(code: str, direction: str, action_type: str = "2", task_id: Optional[str] = None, pos_type: Optional[str] = None, backpack_level: Optional[int] = None, is_target_point: bool = False) -> Dict: """生成导航路径点代码""" path_code = { 'code': code, 'direction': direction, 'type': action_type } if task_id and action_type == "2": path_code['taskId'] = task_id # 只有到达目标点时才添加posType和lev，且有值时才包含字段 if is_target_point: if pos_type: path_code['posType'] = pos_type if backpack_level is not None: path_code['lev'] = backpack_level return path_code