""" 路径规划算法 """ import heapq import random import math import time import logging from typing import Dict, List, Tuple, Optional, Set, Any from collections import defaultdict, deque from common.data_models import PlannedPath, PathCode, AGVStatus, BackpackData from common.utils import get_coordinate_from_path_id, calculate_distance, calculate_manhattan_distance, generate_segment_id, generate_navigation_code from algorithm_system.algorithms.collision_detection import CollisionDetector, CollisionResolver class ExecutingTaskExtractor: """执行中任务提取器""" def __init__(self, path_mapping: Dict[str, Dict[str, int]]): """ 初始化任务提取器 Args: path_mapping: 路径点映射字典 """ self.path_mapping = path_mapping self.logger = logging.getLogger(__name__) # 预计算坐标映射 self.coord_to_code = {(data['x'], data['y']): code for code, data in path_mapping.items()} # 预分类位置类型 self.pickup_positions = [pos for pos in path_mapping.keys() if pos.startswith("1")] self.charging_positions = [pos for pos in path_mapping.keys() if pos.startswith("2")] self.delivery_positions = [pos for pos in path_mapping.keys() if pos.startswith("3")] self.standby_positions = [pos for pos in path_mapping.keys() if pos.startswith("4")] def extract_optimized_executing_tasks(self, agv_status_list: List[AGVStatus]) -> List[Dict]: """智能提取AGV执行任务""" executing_tasks = [] for agv_status in agv_status_list: agv_id = agv_status.agvId current_position = agv_status.position if not agv_status.backpack: continue optimized_task = self._analyze_all_backpack_tasks( agv_id, current_position, agv_status.backpack, agv_status ) if optimized_task: executing_tasks.append(optimized_task) self.logger.info(f"智能分析 {len(executing_tasks)} 个AGV的背篓任务并生成最优路径") return executing_tasks def _analyze_all_backpack_tasks(self, agv_id: str, current_position: str, backpack: List[BackpackData], agv_status: AGVStatus) -> Optional[Dict]: """ 分析所有背篓任务，确定最优执行策略 """ # 1. 提取并分类所有有效任务 loaded_tasks = [] # 已装载的任务 unloaded_tasks = [] # 未装载的任务 all_tasks = [] # 所有任务 self.logger.debug(f"[AGV {agv_id}] 开始分析背篓任务，位置: {current_position}") for i, bp in enumerate(backpack): self.logger.debug(f"[AGV {agv_id}] 背篓位置{i+1}: taskId={bp.taskId}, loaded={bp.loaded}, execute={bp.execute}") if not bp.taskId: self.logger.debug(f"[AGV {agv_id}] 背篓位置{i+1}: 跳过（无任务ID）") continue # 尝试解析任务详情 task_info = self._parse_task_details(bp.taskId, current_position) if not task_info: self.logger.warning(f"[AGV {agv_id}] 任务 {bp.taskId} 解析失败，跳过") continue task_data = { 'backpack_item': bp, 'task_info': task_info, 'distance_to_start': task_info.get('distance_to_start', float('inf')), 'distance_to_end': task_info.get('distance_to_end', float('inf')) } all_tasks.append(task_data) if bp.loaded: loaded_tasks.append(task_data) self.logger.debug(f"[AGV {agv_id}] 任务 {bp.taskId} 已装载，距离终点: {task_info.get('distance_to_end', 'N/A')}") else: unloaded_tasks.append(task_data) self.logger.debug(f"[AGV {agv_id}] 任务 {bp.taskId} 未装载，距离起点: {task_info.get('distance_to_start', 'N/A')}") self.logger.debug(f"[AGV {agv_id}] 任务分析结果: 总任务={len(all_tasks)}, 已装载={len(loaded_tasks)}, 未装载={len(unloaded_tasks)}") # 2. 如果没有任何任务，返回None if not all_tasks: self.logger.debug(f"[AGV {agv_id}] 没有有效任务，返回None") return None # 3. 决策最优路径策略 optimal_strategy = self._determine_next_best_action( loaded_tasks, unloaded_tasks, current_position ) if not optimal_strategy: self.logger.debug(f"[AGV {agv_id}] 决策返回None，没有最优策略") return None self.logger.debug(f"[AGV {agv_id}] 决策完成: {optimal_strategy['strategy']} - {optimal_strategy['action']}") self.logger.debug(f"[AGV {agv_id}] 目标位置: {optimal_strategy['target_position']}") self.logger.debug(f"[AGV {agv_id}] 决策原因: {optimal_strategy.get('reason', 'N/A')}") # 4. 构建任务信息 result = { 'agvId': agv_id, 'currentPosition': current_position, 'strategy': optimal_strategy['strategy'], 'next_action': optimal_strategy['action'], 'target_position': optimal_strategy['target_position'], 'primary_task': optimal_strategy['selected_task'], 'all_loaded_tasks': loaded_tasks, 'all_unloaded_tasks': unloaded_tasks, 'total_tasks_count': len(all_tasks), 'estimated_efficiency': optimal_strategy.get('efficiency_score', 0), 'decision_reason': optimal_strategy.get('reason', ''), 'agvDirection': agv_status.direction, 'agvVoltage': agv_status.vol, 'agvError': agv_status.error } self.logger.debug(f"[AGV {agv_id}] 最终任务信息构建完成，返回结果") return result def _determine_next_best_action(self, loaded_tasks: List[Dict], unloaded_tasks: List[Dict], current_position: str) -> Optional[Dict]: """ 确定AGV下一步最优行动 Args: loaded_tasks: 已装载的任务 unloaded_tasks: 未装载的任务 current_position: 当前位置 Returns: Optional[Dict]: 最优行动策略 """ # 默认策略：综合优化决策 # 1. 如果有已装载任务，优先送货 if loaded_tasks: nearest_delivery = min(loaded_tasks, key=lambda t: t['distance_to_end']) # 2. 如果也有未装载任务，比较距离决策 if unloaded_tasks: nearest_pickup = min(unloaded_tasks, key=lambda t: t['distance_to_start']) # 综合优化策略：如果取货点显著更近，优先取货；否则优先送货 if nearest_pickup['distance_to_start'] * 1.5 < nearest_delivery['distance_to_end']: return { 'strategy': 'comprehensive_optimization', 'action': 'pickup', 'selected_task': nearest_pickup, 'target_position': nearest_pickup['task_info']['start_position'], 'efficiency_score': self._calculate_pickup_efficiency(nearest_pickup, current_position), 'reason': f"综合优化：取货点({nearest_pickup['distance_to_start']})比送货点({nearest_delivery['distance_to_end']})显著更近" } else: return { 'strategy': 'comprehensive_optimization', 'action': 'deliver', 'selected_task': nearest_delivery, 'target_position': nearest_delivery['task_info']['end_position'], 'efficiency_score': self._calculate_delivery_efficiency(nearest_delivery, current_position), 'reason': f"综合优化：优先送达已装载货物，送货距离{nearest_delivery['distance_to_end']}" } else: # 只有已装载任务，直接送货 return { 'strategy': 'comprehensive_optimization', 'action': 'deliver', 'selected_task': nearest_delivery, 'target_position': nearest_delivery['task_info']['end_position'], 'efficiency_score': self._calculate_delivery_efficiency(nearest_delivery, current_position), 'reason': f"综合优化：送达唯一已装载货物，距离{nearest_delivery['distance_to_end']}" } # 3. 如果只有未装载任务，取货 elif unloaded_tasks: nearest_pickup = min(unloaded_tasks, key=lambda t: t['distance_to_start']) return { 'strategy': 'comprehensive_optimization', 'action': 'pickup', 'selected_task': nearest_pickup, 'target_position': nearest_pickup['task_info']['start_position'], 'efficiency_score': self._calculate_pickup_efficiency(nearest_pickup, current_position), 'reason': f"综合优化：取货唯一未装载任务，距离{nearest_pickup['distance_to_start']}" } # 4. 没有任务 return None def _parse_task_details(self, task_id: str, current_position: str) -> Optional[Dict]: """ 解析任务详细信息，包括起终点和距离 Args: task_id: 任务ID current_position: 当前位置 Returns: Optional[Dict]: 任务详细信息 """ self.logger.debug(f"开始解析任务 {task_id}，当前位置: {current_position}") start_pos, end_pos = self._fast_parse_task_start_end(task_id) if not start_pos or not end_pos: self.logger.warning(f"任务 {task_id} 解析失败: start_pos={start_pos}, end_pos={end_pos}") return None self.logger.debug(f"任务 {task_id} 解析成功: start={start_pos}, end={end_pos}") # 计算距离 current_coord = get_coordinate_from_path_id(current_position, self.path_mapping) start_coord = get_coordinate_from_path_id(start_pos, self.path_mapping) end_coord = get_coordinate_from_path_id(end_pos, self.path_mapping) if not all([current_coord, start_coord, end_coord]): return None distance_to_start = calculate_manhattan_distance(current_coord, start_coord) distance_to_end = calculate_manhattan_distance(current_coord, end_coord) return { 'task_id': task_id, 'start_position': start_pos, 'end_position': end_pos, 'distance_to_start': distance_to_start, 'distance_to_end': distance_to_end, 'start_coord': start_coord, 'end_coord': end_coord } def _calculate_delivery_efficiency(self, task_data: Dict, current_position: str) -> float: """ 计算送货效率分数 Args: task_data: 任务数据 current_position: 当前位置 Returns: float: 效率分数 (0-10) """ distance = task_data['distance_to_end'] # 距离越近效率越高 distance_score = max(0, 10 - distance / 5) # 已装载的任务有额外奖励 loaded_bonus = 3.0 if task_data['backpack_item'].loaded else 0 return distance_score + loaded_bonus def _calculate_pickup_efficiency(self, task_data: Dict, current_position: str) -> float: """ 计算取货效率分数 Args: task_data: 任务数据 current_position: 当前位置 Returns: float: 效率分数 (0-10) """ distance_to_start = task_data['distance_to_start'] # 距离越近效率越高 distance_score = max(0, 10 - distance_to_start / 3) # 很近的取货点有高奖励 if distance_to_start <= 3: distance_score += 5.0 elif distance_to_start <= 5: distance_score += 2.0 return distance_score def _fast_parse_task_start_end(self, task_id: str) -> Tuple[Optional[str], Optional[str]]: """ 解析任务起点和终点 支持两种格式： 1. 标准格式：xxx_start_end 2. 简单格式：数字ID（通过hash分配位置） Args: task_id: 任务ID Returns: Tuple[Optional[str], Optional[str]]: (起点, 终点) """ self.logger.debug(f"解析任务ID: {task_id}") # 方法1：尝试标准格式解析（包含下划线） if "_" in task_id: parts = task_id.split("_") self.logger.debug(f"任务ID包含下划线，分割结果: {parts}") if len(parts) >= 3: potential_start = parts[-2] potential_end = parts[-1] start_valid = potential_start in self.path_mapping end_valid = potential_end in self.path_mapping self.logger.debug(f"标准格式解析结果: start={potential_start}(valid={start_valid}), end={potential_end}(valid={end_valid})") if start_valid and end_valid: return potential_start, potential_end self.logger.debug(f"任务ID分割后部分数量不足: {len(parts)} < 3") # 方法2：简单数字ID的备选机制（通过hash分配位置） self.logger.debug(f"尝试简单数字ID备选解析机制") # 获取所有可用位置 all_positions = list(self.path_mapping.keys()) if len(all_positions) < 2: self.logger.warning(f"路径映射位置不足，无法为任务分配起终点") return None, None # 使用任务ID的hash值来分配起点和终点 try: task_hash = abs(hash(task_id)) # 分配起点（使用hash的前一半） start_index = task_hash % len(all_positions) start_pos = all_positions[start_index] # 分配终点（使用hash的后一半，确保与起点不同） end_index = (task_hash // len(all_positions)) % len(all_positions) if end_index == start_index: end_index = (end_index + 1) % len(all_positions) end_pos = all_positions[end_index] self.logger.debug(f"简单ID备选解析成功: 任务{task_id} -> start={start_pos}, end={end_pos}") return start_pos, end_pos except Exception as e: self.logger.error(f"简单ID备选解析失败: {e}") self.logger.warning(f"任务ID {task_id} 所有解析方法都失败，返回 None, None") return None, None def _fast_parse_target(self, task_id: str, current_position: str, loaded: bool) -> Optional[str]: """ 解析任务目标位置 Args: task_id: 任务ID current_position: 当前位置 loaded: 是否已装载 Returns: Optional[str]: 目标位置码 """ _, task_end = self._fast_parse_task_start_end(task_id) if task_end: return task_end # 基于hash值和装载状态快速选择 hash_val = hash(task_id) % 1000 if loaded: # 已装载，选择出库区域 target_positions = [pos for pos in self.delivery_positions if pos != current_position] if not target_positions: target_positions = [pos for pos in self.path_mapping.keys() if pos != current_position] else: # 未装载，选择仓库区域 target_positions = [pos for pos in self.pickup_positions if pos != current_position] if not target_positions: target_positions = [pos for pos in self.path_mapping.keys() if pos != current_position] if target_positions: return target_positions[hash_val % len(target_positions)] return None def extract_executing_tasks(self, agv_status_list: List[AGVStatus]) -> List[Dict]: """提取所有AGV正在执行的任务""" self.logger.debug(f"开始转换任务为标准格式，AGV数量: {len(agv_status_list)}") optimized_tasks = self.extract_optimized_executing_tasks(agv_status_list) self.logger.debug(f"提取结果数量: {len(optimized_tasks)}") legacy_tasks = [] for i, task in enumerate(optimized_tasks): try: self.logger.debug(f"转换任务 #{i+1}: AGV {task['agvId']}") primary_task = task['primary_task'] task_info = primary_task['task_info'] backpack_item = primary_task['backpack_item'] self.logger.debug(f" - primary_task存在: {primary_task is not None}") self.logger.debug(f" - task_info存在: {task_info is not None}") self.logger.debug(f" - backpack_item存在: {backpack_item is not None}") # 根据action确定状态 is_pickup_action = task['next_action'] == 'pickup' self.logger.debug(f" - next_action: {task['next_action']}") self.logger.debug(f" - is_pickup_action: {is_pickup_action}") self.logger.debug(f" - target_position: {task['target_position']}") # 保持原有的标准JSON格式 legacy_task = { 'agvId': task['agvId'], 'taskId': task_info['task_id'], 'currentPosition': task['currentPosition'], 'backpackIndex': backpack_item.index, 'status': 'executing', 'agvDirection': task['agvDirection'], 'agvVoltage': task['agvVoltage'], 'agvError': task['agvError'], 'loaded': backpack_item.loaded, 'needPickup': is_pickup_action, 'targetPosition': task['target_position'], 'taskStart': task_info['start_position'], 'taskEnd': task_info['end_position'] } self.logger.debug(f" - 任务构建成功: {legacy_task['agvId']} -> {legacy_task['targetPosition']}") legacy_tasks.append(legacy_task) except Exception as e: self.logger.error(f"转换任务 #{i+1} 失败: {e}") self.logger.error(f"任务内容: {task}") continue self.logger.info(f"使用综合优化策略，为 {len(legacy_tasks)} 个AGV生成标准格式路径") return legacy_tasks class BatchPathPlanner: """批量路径规划器""" def __init__(self, path_mapping: Dict[str, Dict[str, int]], algorithm_type: str = "A_STAR", agv_manager=None): """ 初始化批量路径规划器 Args: path_mapping: 路径点映射字典 algorithm_type: 基础路径规划算法类型 agv_manager: 用于获取AGV状态信息 """ self.path_mapping = path_mapping self.algorithm_type = algorithm_type self.agv_manager = agv_manager self.logger = logging.getLogger(__name__) # 初始化优化组件 self.task_extractor = ExecutingTaskExtractor(path_mapping) self.collision_detector = FastCollisionDetector(min_distance=3.0) self.collision_resolver = FastCollisionResolver(path_mapping, self.collision_detector, agv_manager) self.base_planner = PathPlanningFactory.create_path_planner(algorithm_type, path_mapping) def plan_all_agv_paths(self, agv_status_list: List[AGVStatus], include_idle_agv: bool = False, constraints: List[Tuple[int, int, float]] = None) -> Dict: """ 为所有AGV规划路径（每个AGV只生成一个路径） Args: agv_status_list: AGV状态列表 include_idle_agv: 是否包含空闲AGV constraints: 路径约束条件 Returns: Dict: 包含所有路径信息的结果 """ start_time = time.time() # 1. 提取执行中的任务（使用向后兼容的方法） executing_tasks = self.task_extractor.extract_executing_tasks(agv_status_list) # 2. 路径规划 planned_paths = [] total_planning_time = 0 planned_agv_ids = set() for task in executing_tasks: agv_id = task['agvId'] current_pos = task['currentPosition'] target_pos = task['targetPosition'] task_id = task['taskId'] self.logger.debug(f"开始路径规划: AGV {agv_id}, 任务 {task_id}") self.logger.debug(f" - current_pos: {current_pos} ({type(current_pos)})") self.logger.debug(f" - target_pos: {target_pos} ({type(target_pos)})") # 避免重复规划 if agv_id in planned_agv_ids: self.logger.debug(f" - 跳过AGV {agv_id}：已规划") continue # 验证位置格式 if not current_pos or not target_pos: self.logger.error(f" - AGV {agv_id} 位置信息无效: current={current_pos}, target={target_pos}") continue # 路径格式标准化 from common.utils import normalize_path_id normalized_current = normalize_path_id(current_pos) normalized_target = normalize_path_id(target_pos) self.logger.debug(f" - 格式标准化: {current_pos} -> {normalized_current}, {target_pos} -> {normalized_target}") # 路径规划 path_start_time = time.time() self.logger.debug(f" - 调用base_planner.plan_path({normalized_current}, {normalized_target})") planned_path = self.base_planner.plan_path(normalized_current, normalized_target, constraints) path_end_time = time.time() planning_time = (path_end_time - path_start_time) * 1000 total_planning_time += planning_time self.logger.debug(f" - 路径规划结果: {planned_path is not None}") if planned_path: self.logger.debug(f" - 路径长度: {len(planned_path.codeList) if hasattr(planned_path, 'codeList') else 'N/A'}") else: self.logger.error(f" - AGV {agv_id} 路径规划失败: {current_pos} -> {target_pos}") if planned_path: # 生成路径字典 seg_id = generate_segment_id(agv_id=agv_id, task_id=task_id, action_type="2") path_dict = { 'agvId': agv_id, 'segId': seg_id, 'codeList': [] } # 转换路径点格式 for i, path_code in enumerate(planned_path.codeList): if isinstance(path_code, PathCode): action_type = "2" # 任务类型 pos_type = None backpack_level = task.get('backpackIndex', 0) is_target_point = False # 确定位置类型：最后一个点的动作类型 if i == len(planned_path.codeList) - 1: # 最后一个路径点 is_target_point = True if task.get('loaded', False): # 已装载任务：当前路径是去终点放货 pos_type = "2" # 放货 self.logger.debug(f"AGV {agv_id} 在终点 {path_code.code} 设置放货动作") else: # 未装载任务：当前路径是去起点取货 pos_type = "1" # 取货 self.logger.debug(f"AGV {agv_id} 在起点 {path_code.code} 设置取货动作") enhanced_code = generate_navigation_code( code=path_code.code, direction=path_code.direction, action_type=action_type, task_id=task_id, pos_type=pos_type, backpack_level=backpack_level, is_target_point=is_target_point ) path_dict['codeList'].append(enhanced_code) else: enhanced_code = generate_navigation_code( code=path_code.get('code', ''), direction=path_code.get('direction', '90'), action_type="2", task_id=task_id ) path_dict['codeList'].append(enhanced_code) planned_paths.append(path_dict) planned_agv_ids.add(agv_id) self.logger.debug(f"AGV {agv_id} 快速规划: {current_pos} -> {target_pos}") # 3. 空闲AGV处理 if include_idle_agv: idle_paths = self._fast_plan_idle_agv_paths(agv_status_list, executing_tasks, constraints, planned_agv_ids) planned_paths.extend(idle_paths) # 4. 去重检查 final_paths = [] final_agv_ids = set() for path in planned_paths: agv_id = path['agvId'] if agv_id not in final_agv_ids: final_paths.append(path) final_agv_ids.add(agv_id) planned_paths = final_paths # 5. 碰撞检测和解决 conflicts = self.collision_detector.detect_conflicts(planned_paths) if conflicts: self.logger.info(f"发现 {len(conflicts)} 个碰撞，快速解决中...") planned_paths = self.collision_resolver.resolve_conflicts(planned_paths, conflicts, executing_tasks) remaining_conflicts = self.collision_detector.detect_conflicts(planned_paths) else: remaining_conflicts = [] end_time = time.time() total_time = (end_time - start_time) * 1000 # 6. 构建优化结果 result = { 'totalAgvs': len(agv_status_list), 'executingTasksCount': len(executing_tasks), 'plannedPathsCount': len(planned_paths), 'totalPlanningTime': round(total_time, 2), 'pathPlanningTime': round(total_planning_time, 2), 'conflictsDetected': len(conflicts), 'remainingConflicts': len(remaining_conflicts), 'algorithm': f"Optimized_{self.algorithm_type}", 'fourDirectionCompliant': True, 'collisionFree': len(remaining_conflicts) == 0, 'plannedPaths': planned_paths, 'executingTasks': executing_tasks } self.logger.info(f"优化批量路径规划完成 - 总AGV: {result['totalAgvs']}, " f"执行中任务: {result['executingTasksCount']}, " f"规划路径: {result['plannedPathsCount']}, " f"总耗时: {result['totalPlanningTime']:.2f}ms, " f"无碰撞: {result['collisionFree']}") return result def _fast_plan_idle_agv_paths(self, agv_status_list: List[AGVStatus], executing_tasks: List[Dict], constraints: List[Tuple[int, int, float]] = None, planned_agv_ids: Set[str] = None) -> List[Dict]: """ 为空闲AGV规划路径 """ idle_paths = [] executing_agv_ids = {task['agvId'] for task in executing_tasks} if planned_agv_ids is None: planned_agv_ids = executing_agv_ids.copy() for agv_status in agv_status_list: agv_id = agv_status.agvId if agv_id in planned_agv_ids: continue if self._is_agv_idle_fast(agv_status): current_pos = agv_status.position target_pos = self._get_idle_agv_target_fast(agv_id, current_pos) if target_pos and target_pos != current_pos: planned_path = self.base_planner.plan_path(current_pos, target_pos, constraints) if planned_path: action_type = "3" if self._is_charging_path(target_pos) else "4" seg_id = generate_segment_id(agv_id=agv_id, target_position=target_pos, action_type=action_type) path_dict = { 'agvId': agv_id, 'segId': seg_id, 'codeList': [] } for i, path_code in enumerate(planned_path.codeList): if isinstance(path_code, PathCode): # 空闲AGV不需要taskId、posType和lev字段 enhanced_code = generate_navigation_code( code=path_code.code, direction=path_code.direction, action_type=action_type, is_target_point=False # 空闲AGV路径无目标点概念 ) path_dict['codeList'].append(enhanced_code) idle_paths.append(path_dict) planned_agv_ids.add(agv_id) return idle_paths def _is_agv_idle_fast(self, agv_status: AGVStatus) -> bool: """判断AGV是否空闲""" if agv_status.status not in [0, 1, 2] or agv_status.error != 0: return False # 检查背篓 if agv_status.backpack: for item in agv_status.backpack[:2]: # 只检查前两个 if item.loaded and item.execute: return False return True def _get_idle_agv_target_fast(self, agv_id: str, current_position: str) -> Optional[str]: """获取空闲AGV目标位置""" hash_val = hash(agv_id) % 1000 # 选择逻辑 charging_positions = [pos for pos in self.task_extractor.charging_positions if pos != current_position] if charging_positions: return charging_positions[hash_val % len(charging_positions)] standby_positions = [pos for pos in self.task_extractor.standby_positions if pos != current_position] if standby_positions: return standby_positions[hash_val % len(standby_positions)] all_positions = [pos for pos in self.path_mapping.keys() if pos != current_position] if all_positions: return all_positions[hash_val % len(all_positions)] return None def _is_charging_path(self, target_position: str) -> bool: """ 判断目标位置是否为充电站（后期修改） """ if not target_position: return False # 充电站识别 return target_position.startswith("2") or "charge" in target_position.lower() class PathPlanner: """路径规划器基类""" def __init__(self, path_mapping: Dict[str, Dict[str, int]]): """ 初始化路径规划器 Args: path_mapping: 路径点映射字典 """ self.path_mapping = path_mapping self.logger = logging.getLogger(__name__) def plan_path(self, start_code: str, end_code: str, constraints: List[Tuple[int, int, float]] = None) -> Optional[PlannedPath]: """ 规划从起点到终点的路径 Args: start_code: 起始路径点ID end_code: 目标路径点ID constraints: 约束条件列表 Returns: Optional[PlannedPath]: 规划的路径，如果无法规划则返回None """ raise NotImplementedError("子类必须实现此方法") def is_valid_path_point(self, code: str) -> bool: """ 检查路径点ID是否有效 Args: code: 路径点ID Returns: bool: 是否有效 """ return code in self.path_mapping class AStarPathPlanner(PathPlanner): """A*路径规划器""" def __init__(self, path_mapping: Dict[str, Dict[str, int]], turn_cost: float = 0.5): """ 初始化A*路径规划器 Args: path_mapping: 路径点映射字典 turn_cost: 转向代价 """ super().__init__(path_mapping) self.turn_cost = turn_cost self.logger = logging.getLogger(__name__) # 四个方向：上、右、下、左 (严格四方向移动) self.directions = [(0, -1), (1, 0), (0, 1), (-1, 0)] self.direction_angles = ["270", "0", "90", "180"] # 预计算坐标映射（必须在 _build_adjacency_list 之前初始化） self.coord_to_code = {} for code, data in path_mapping.items(): if isinstance(data, dict) and 'x' in data and 'y' in data: self.coord_to_code[(data['x'], data['y'])] = code # 预计算邻接表 self.adjacency = self._build_adjacency_list() # 性能监控 self.planning_count = 0 self.total_planning_time = 0.0 self.logger.debug(f"A*规划器初始化完成，邻接表: {len(self.adjacency)} 个节点") def _build_adjacency_list(self) -> Dict[str, List[Tuple[str, str]]]: """ 预计算邻接表 Returns: Dict[str, List[Tuple[str, str]]]: {节点: [(邻居节点, 移动方向), ...]} """ adjacency = defaultdict(list) for code, coord_data in self.path_mapping.items(): x, y = coord_data['x'], coord_data['y'] # 检查四个方向的邻居 for i, (dx, dy) in enumerate(self.directions): next_x, next_y = x + dx, y + dy # 查找邻居代码 neighbor_coord = (next_x, next_y) if neighbor_coord in self.coord_to_code: neighbor_code = self.coord_to_code[neighbor_coord] direction = self.direction_angles[i] adjacency[code].append((neighbor_code, direction)) return adjacency def plan_path(self, start_code: str, end_code: str, constraints: List[Tuple[int, int, float]] = None) -> Optional[PlannedPath]: """ Args: start_code: 起始路径点ID end_code: 目标路径点ID constraints: 约束条件列表 Returns: Optional[PlannedPath]: 规划的路径 """ # 验证 if not self.is_valid_path_point(start_code) or not self.is_valid_path_point(end_code): return None if start_code == end_code: return PlannedPath(agvId="", codeList=[PathCode(code=start_code, direction="90")]) # 获取坐标（使用预计算映射） start_coord = (self.path_mapping[start_code]['x'], self.path_mapping[start_code]['y']) end_coord = (self.path_mapping[end_code]['x'], self.path_mapping[end_code]['y']) # A*实现 # 使用元组堆，减少对象创建开销 open_set = [(0, start_code, None, [])] # (f_score, code, parent, path) closed_set = set() g_scores = {start_code: 0} # 预计算约束检查函数 constraint_check = self._build_constraint_checker(constraints) if constraints else None while open_set: f_score, current_code, parent_code, path = heapq.heappop(open_set) if current_code in closed_set: continue closed_set.add(current_code) current_path = path + [current_code] if current_code == end_code: # 重建路径 return self._build_path_result(current_path) # 使用预计算的邻接表 for neighbor_code, direction in self.adjacency[current_code]: if neighbor_code in closed_set: continue # 约束检查 if constraint_check and constraint_check(neighbor_code): continue # 代价计算 g_cost = g_scores[current_code] + 1 if neighbor_code not in g_scores or g_cost < g_scores[neighbor_code]: g_scores[neighbor_code] = g_cost # 启发式计算 neighbor_coord = (self.path_mapping[neighbor_code]['x'], self.path_mapping[neighbor_code]['y']) h_cost = abs(neighbor_coord[0] - end_coord[0]) + abs(neighbor_coord[1] - end_coord[1]) f_cost = g_cost + h_cost heapq.heappush(open_set, (f_cost, neighbor_code, current_code, current_path)) return None def _build_constraint_checker(self, constraints: List[Tuple[int, int, float]]): """构建约束检查函数""" def check_constraints(code: str) -> bool: coord = (self.path_mapping[code]['x'], self.path_mapping[code]['y']) for cx, cy, radius in constraints: if (coord[0] - cx) ** 2 + (coord[1] - cy) ** 2 <= radius ** 2: return True return False return check_constraints def _build_path_result(self, path_codes: List[str]) -> PlannedPath: """构建路径结果""" result_codes = [] for i, code in enumerate(path_codes): direction = "90" # 默认方向 if i < len(path_codes) - 1: # 使用预计算的邻接表查找方向 next_code = path_codes[i + 1] for neighbor_code, neighbor_direction in self.adjacency[code]: if neighbor_code == next_code: direction = neighbor_direction break result_codes.append(PathCode(code=code, direction=direction)) return PlannedPath(agvId="", codeList=result_codes) # 碰撞检测器 class FastCollisionDetector: """碰撞检测器""" def __init__(self, min_distance: float = 3.0): self.min_distance = min_distance self.logger = logging.getLogger(__name__) def detect_conflicts(self, planned_paths: List[Dict]) -> List[Dict]: """ 碰撞检测 - 基于时间步的优化检测 Args: planned_paths: 规划路径列表 Returns: List[Dict]: 冲突列表 """ conflicts = [] if len(planned_paths) < 2: return conflicts # 构建时间-位置映射 time_positions = defaultdict(list) for path_data in planned_paths: agv_id = path_data['agvId'] code_list = path_data.get('codeList', []) for time_step, path_code in enumerate(code_list): # 快速提取位置码 if isinstance(path_code, dict): position = path_code.get('code', '') elif hasattr(path_code, 'code'): position = path_code.code else: position = str(path_code) if position: time_positions[time_step].append((agv_id, position)) # 快速冲突检测 for time_step, agv_positions in time_positions.items(): if len(agv_positions) < 2: continue # 位置分组检查 position_groups = defaultdict(list) for agv_id, position in agv_positions: position_groups[position].append(agv_id) # 发现冲突 for position, agv_list in position_groups.items(): if len(agv_list) > 1: conflicts.append({ 'type': 'position_conflict', 'time_step': time_step, 'position': position, 'agv_ids': agv_list, 'severity': 'high' }) self.logger.debug(f"快速检测到 {len(conflicts)} 个冲突") return conflicts # 碰撞解决器 class FastCollisionResolver: """碰撞解决器""" def __init__(self, path_mapping: Dict[str, Dict[str, int]], collision_detector: FastCollisionDetector, agv_manager=None): self.path_mapping = path_mapping self.collision_detector = collision_detector self.agv_manager = agv_manager self.logger = logging.getLogger(__name__) def resolve_conflicts(self, planned_paths: List[Dict], conflicts: List[Dict], executing_tasks: List[Dict] = None) -> List[Dict]: """ 解决冲突 Args: planned_paths: 规划路径列表 conflicts: 冲突列表 executing_tasks: 执行任务列表 Returns: List[Dict]: 解决冲突后的路径列表 """ if not conflicts: return planned_paths # 构建路径字典便于快速访问 paths_dict = {path['agvId']: path for path in planned_paths} # 按时间步排序处理冲突 sorted_conflicts = sorted(conflicts, key=lambda x: x.get('time_step', 0)) for conflict in sorted_conflicts: if conflict['type'] == 'position_conflict': agv_ids = conflict['agv_ids'] time_step = conflict['time_step'] # 简单策略：保留第一个AGV，其他延迟 sorted_agv_ids = sorted(agv_ids) for i, agv_id in enumerate(sorted_agv_ids[1:], 1): if agv_id in paths_dict: self._add_delay_to_path(paths_dict[agv_id], delay_steps=i) self.logger.debug(f"为AGV {agv_id} 添加 {i} 步延迟") return list(paths_dict.values()) def _add_delay_to_path(self, path_data: Dict, delay_steps: int): """为路径添加延迟步骤""" if delay_steps <= 0 or not path_data.get('codeList'): return # 在路径开始处重复第一个位置 first_code = path_data['codeList'][0] # 创建延迟代码 if isinstance(first_code, dict): delay_code = first_code.copy() else: delay_code = { 'code': first_code.code if hasattr(first_code, 'code') else str(first_code), 'direction': first_code.direction if hasattr(first_code, 'direction') else "90" } # 添加延迟步骤 for _ in range(delay_steps): path_data['codeList'].insert(0, delay_code) def validate_four_direction_movement(self, planned_paths: List[Dict]) -> List[Dict]: """验证四向移动约束""" return planned_paths class PathPlanningFactory: """路径规划器工厂类""" @staticmethod def create_path_planner(algorithm_type: str, path_mapping: Dict[str, Dict[str, int]]) -> PathPlanner: """ 创建路径规划器实例 Args: algorithm_type: 算法类型 path_mapping: 路径点映射字典 Returns: PathPlanner: 路径规划器实例 """ algorithm_upper = algorithm_type.upper() if algorithm_upper in ["A_STAR", "ASTAR", "A*", "DIJKSTRA", "GREEDY"]: return AStarPathPlanner(path_mapping) else: # 默认A*算法 logging.getLogger(__name__).warning(f"未知算法类型 {algorithm_type}，使用默认A*算法") return AStarPathPlanner(path_mapping) @staticmethod def create_batch_path_planner(algorithm_type: str, path_mapping: Dict[str, Dict[str, int]], agv_manager=None) -> BatchPathPlanner: """ 创建批量路径规划器实例 Args: algorithm_type: 基础算法类型 path_mapping: 路径点映射字典 agv_manager: AGV管理器，用于获取AGV状态信息 Returns: BatchPathPlanner: 批量路径规划器实例 """ return BatchPathPlanner(path_mapping, algorithm_type, agv_manager) # 导出核心类和函数 __all__ = [ 'ExecutingTaskExtractor', 'BatchPathPlanner', 'PathPlanner', 'AStarPathPlanner', 'PathPlanningFactory', 'FastCollisionDetector', 'FastCollisionResolver' ]