"""
|
AGV路径碰撞检测和解决模块
|
"""
|
import math
|
import logging
|
from typing import Dict, List, Tuple, Optional, Set
|
from dataclasses import dataclass
|
from collections import defaultdict
|
|
from common.data_models import PlannedPath, PathCode, AGVActionTypeEnum
|
from common.utils import get_coordinate_from_path_id
|
|
|
@dataclass
|
class SpaceTimeNode:
|
"""时空节点 - 表示AGV在特定时间和位置的状态"""
|
agv_id: str
|
position: str # 位置码
|
coordinates: Tuple[int, int] # 坐标
|
time_step: int # 时间步
|
direction: str # 方向
|
|
|
@dataclass
|
class Conflict:
|
"""冲突描述"""
|
type: str # 冲突类型: "vertex", "edge", "follow"
|
agv1: str
|
agv2: str
|
time_step: int
|
position1: str
|
position2: str
|
description: str
|
|
|
@dataclass
|
class AGVPriority:
|
"""AGV优先级评估结果"""
|
agv_id: str
|
priority_score: float # 优先级分数，越高越优先
|
task_status: str # 任务状态
|
action_type: str # 动作类型
|
task_priority: int # 任务优先级
|
voltage: int # 电量
|
explanation: str # 优先级说明
|
|
|
class CollisionDetector:
|
"""AGV路径碰撞检测器"""
|
|
def __init__(self, path_mapping: Dict[str, Dict[str, int]],
|
min_distance: float = 3.0, time_buffer: int = 1):
|
"""
|
初始化碰撞检测器
|
|
Args:
|
path_mapping: 路径点映射字典
|
min_distance: AGV之间的最小安全距离
|
time_buffer: 时间缓冲区（时间步数）
|
"""
|
self.path_mapping = path_mapping
|
self.min_distance = min_distance
|
self.time_buffer = time_buffer
|
self.logger = logging.getLogger(__name__)
|
|
def detect_conflicts(self, planned_paths: List[Dict]) -> List[Conflict]:
|
"""
|
检测所有AGV路径之间的冲突
|
|
Args:
|
planned_paths: 规划路径列表
|
|
Returns:
|
List[Conflict]: 冲突列表
|
"""
|
conflicts = []
|
|
# 构建时空表
|
space_time_table = self._build_space_time_table(planned_paths)
|
|
# 检测顶点冲突（同一时间同一位置）
|
conflicts.extend(self._detect_vertex_conflicts(space_time_table))
|
|
# 检测边冲突（AGV交换位置）
|
conflicts.extend(self._detect_edge_conflicts(space_time_table))
|
|
# 检测跟随冲突（AGV距离过近）
|
conflicts.extend(self._detect_following_conflicts(space_time_table))
|
|
self.logger.info(f"检测到 {len(conflicts)} 个路径冲突")
|
return conflicts
|
|
def _build_space_time_table(self, planned_paths: List[Dict]) -> Dict[int, List[SpaceTimeNode]]:
|
"""
|
构建时空表
|
|
Args:
|
planned_paths: 规划路径列表
|
|
Returns:
|
Dict[int, List[SpaceTimeNode]]: 时间步 -> 时空节点列表
|
"""
|
space_time_table = defaultdict(list)
|
|
for path_data in planned_paths:
|
agv_id = path_data.get('agvId', '')
|
code_list = path_data.get('codeList', [])
|
|
for time_step, path_code in enumerate(code_list):
|
position = path_code.get('code', '') if isinstance(path_code, dict) else path_code.code
|
direction = path_code.get('direction', '90') if isinstance(path_code, dict) else path_code.direction
|
|
coordinates = get_coordinate_from_path_id(position, self.path_mapping)
|
if coordinates:
|
node = SpaceTimeNode(
|
agv_id=agv_id,
|
position=position,
|
coordinates=coordinates,
|
time_step=time_step,
|
direction=direction
|
)
|
space_time_table[time_step].append(node)
|
|
return space_time_table
|
|
def _detect_vertex_conflicts(self, space_time_table: Dict[int, List[SpaceTimeNode]]) -> List[Conflict]:
|
"""检测顶点冲突（同一时间同一位置）"""
|
conflicts = []
|
|
for time_step, nodes in space_time_table.items():
|
# 按位置分组
|
position_groups = defaultdict(list)
|
for node in nodes:
|
position_groups[node.position].append(node)
|
|
# 检查每个位置是否有多个AGV
|
for position, agv_nodes in position_groups.items():
|
if len(agv_nodes) > 1:
|
# 发现冲突
|
for i in range(len(agv_nodes)):
|
for j in range(i + 1, len(agv_nodes)):
|
conflict = Conflict(
|
type="vertex",
|
agv1=agv_nodes[i].agv_id,
|
agv2=agv_nodes[j].agv_id,
|
time_step=time_step,
|
position1=position,
|
position2=position,
|
description=f"AGV {agv_nodes[i].agv_id} 和 {agv_nodes[j].agv_id} 在时间 {time_step} 占用同一位置 {position}"
|
)
|
conflicts.append(conflict)
|
|
return conflicts
|
|
def _detect_edge_conflicts(self, space_time_table: Dict[int, List[SpaceTimeNode]]) -> List[Conflict]:
|
"""检测边冲突（AGV交换位置）"""
|
conflicts = []
|
|
max_time = max(space_time_table.keys()) if space_time_table else 0
|
|
for time_step in range(max_time):
|
current_nodes = space_time_table.get(time_step, [])
|
next_nodes = space_time_table.get(time_step + 1, [])
|
|
# 构建当前和下一时刻的位置映射
|
current_positions = {node.agv_id: node.position for node in current_nodes}
|
next_positions = {node.agv_id: node.position for node in next_nodes}
|
|
# 检查是否有AGV交换位置
|
for agv1, pos1_current in current_positions.items():
|
for agv2, pos2_current in current_positions.items():
|
if agv1 >= agv2: # 避免重复检查
|
continue
|
|
pos1_next = next_positions.get(agv1)
|
pos2_next = next_positions.get(agv2)
|
|
if (pos1_next and pos2_next and
|
pos1_current == pos2_next and pos2_current == pos1_next):
|
# 发现边冲突
|
conflict = Conflict(
|
type="edge",
|
agv1=agv1,
|
agv2=agv2,
|
time_step=time_step,
|
position1=pos1_current,
|
position2=pos2_current,
|
description=f"AGV {agv1} 和 {agv2} 在时间 {time_step}-{time_step+1} 交换位置 {pos1_current}<->{pos2_current}"
|
)
|
conflicts.append(conflict)
|
|
return conflicts
|
|
def _detect_following_conflicts(self, space_time_table: Dict[int, List[SpaceTimeNode]]) -> List[Conflict]:
|
"""检测跟随冲突（AGV距离过近）"""
|
conflicts = []
|
|
for time_step, nodes in space_time_table.items():
|
# 检查所有AGV对之间的距离
|
for i in range(len(nodes)):
|
for j in range(i + 1, len(nodes)):
|
node1, node2 = nodes[i], nodes[j]
|
|
# 计算欧几里得距离
|
distance = math.sqrt(
|
(node1.coordinates[0] - node2.coordinates[0]) ** 2 +
|
(node1.coordinates[1] - node2.coordinates[1]) ** 2
|
)
|
|
if distance < self.min_distance and distance > 0:
|
conflict = Conflict(
|
type="follow",
|
agv1=node1.agv_id,
|
agv2=node2.agv_id,
|
time_step=time_step,
|
position1=node1.position,
|
position2=node2.position,
|
description=f"AGV {node1.agv_id} 和 {node2.agv_id} 在时间 {time_step} 距离过近 ({distance:.2f} < {self.min_distance})"
|
)
|
conflicts.append(conflict)
|
|
return conflicts
|
|
|
class CollisionResolver:
|
"""AGV路径碰撞解决器"""
|
|
def __init__(self, path_mapping: Dict[str, Dict[str, int]], detector: CollisionDetector, agv_manager=None):
|
"""
|
初始化碰撞解决器
|
|
Args:
|
path_mapping: 路径点映射字典
|
detector: 碰撞检测器
|
agv_manager: AGV管理器，用于获取AGV状态信息
|
"""
|
self.path_mapping = path_mapping
|
self.detector = detector
|
self.agv_manager = agv_manager
|
self.logger = logging.getLogger(__name__)
|
|
def evaluate_agv_priority(self, agv_id: str, path: Dict, executing_tasks: List[Dict] = None) -> AGVPriority:
|
"""
|
评估AGV的避让优先级
|
|
Args:
|
agv_id: AGV ID
|
path: AGV路径信息
|
executing_tasks: 执行中的任务列表
|
|
Returns:
|
AGVPriority: AGV优先级评估结果
|
"""
|
# 默认值
|
task_status = "idle"
|
action_type = "unknown"
|
task_priority = 1
|
voltage = 100
|
priority_score = 0.0
|
explanation_parts = []
|
|
# 获取AGV状态信息
|
agv_model = None
|
if self.agv_manager:
|
agv_model = self.agv_manager.get_agv_by_id(agv_id)
|
if agv_model:
|
voltage = agv_model.voltage
|
|
# 从路径信息中获取动作类型
|
code_list = path.get('codeList', [])
|
if code_list:
|
first_code = code_list[0]
|
if isinstance(first_code, dict):
|
action_type = first_code.get('type', 'unknown')
|
|
# 从执行中任务获取任务状态和优先级
|
if executing_tasks:
|
for task in executing_tasks:
|
if task.get('agvId') == agv_id:
|
task_status = task.get('status', 'idle')
|
# 尝试从任务ID获取优先级（简化处理）
|
task_id = task.get('taskId', '')
|
if 'HIGH_PRIORITY' in task_id:
|
task_priority = 10
|
elif 'PRIORITY' in task_id:
|
task_priority = 8
|
else:
|
task_priority = 5
|
break
|
|
# 计算优先级分数（分数越高优先级越高，越不容易让步）
|
priority_score = 0.0
|
|
# 1. 任务状态优先级 (40%权重)
|
if task_status == "executing":
|
priority_score += 40.0
|
explanation_parts.append("执行任务中(+40)")
|
elif task_status == "assigned":
|
priority_score += 20.0
|
explanation_parts.append("已分配任务(+20)")
|
else:
|
priority_score += 5.0
|
explanation_parts.append("空闲状态(+5)")
|
|
# 2. 动作类型优先级 (30%权重)
|
if action_type == AGVActionTypeEnum.TASK.value:
|
priority_score += 30.0
|
explanation_parts.append("任务行为(+30)")
|
elif action_type == AGVActionTypeEnum.CHARGING.value:
|
priority_score += 25.0
|
explanation_parts.append("充电行为(+25)")
|
elif action_type == AGVActionTypeEnum.AVOIDANCE.value:
|
priority_score += 5.0
|
explanation_parts.append("避让行为(+5)")
|
elif action_type == AGVActionTypeEnum.STANDBY.value:
|
priority_score += 10.0
|
explanation_parts.append("待机行为(+10)")
|
else:
|
priority_score += 15.0
|
explanation_parts.append("未知行为(+15)")
|
|
# 3. 任务优先级 (20%权重)
|
task_priority_score = (task_priority / 10.0) * 20.0
|
priority_score += task_priority_score
|
explanation_parts.append(f"任务优先级{task_priority}(+{task_priority_score:.1f})")
|
|
# 4. 电量状态 (10%权重)
|
if voltage <= 10: # 必须充电
|
priority_score += 10.0
|
explanation_parts.append("电量危险(+10)")
|
elif voltage <= 20: # 可自动充电
|
priority_score += 8.0
|
explanation_parts.append("电量偏低(+8)")
|
elif voltage <= 50:
|
priority_score += 5.0
|
explanation_parts.append("电量一般(+5)")
|
else:
|
priority_score += 2.0
|
explanation_parts.append("电量充足(+2)")
|
|
explanation = f"总分{priority_score:.1f}: " + ", ".join(explanation_parts)
|
|
return AGVPriority(
|
agv_id=agv_id,
|
priority_score=priority_score,
|
task_status=task_status,
|
action_type=action_type,
|
task_priority=task_priority,
|
voltage=voltage,
|
explanation=explanation
|
)
|
|
def resolve_conflicts(self, planned_paths: List[Dict], conflicts: List[Conflict], executing_tasks: List[Dict] = None) -> List[Dict]:
|
"""
|
解决冲突
|
|
Args:
|
planned_paths: 原始规划路径
|
conflicts: 冲突列表
|
executing_tasks: 执行中的任务列表
|
|
Returns:
|
List[Dict]: 解决冲突后的路径
|
"""
|
if not conflicts:
|
return planned_paths
|
|
resolved_paths = [path.copy() for path in planned_paths]
|
|
# 按时间步排序冲突，优先解决早期冲突
|
conflicts_sorted = sorted(conflicts, key=lambda c: c.time_step)
|
|
for conflict in conflicts_sorted:
|
resolved_paths = self._resolve_single_conflict(resolved_paths, conflict, executing_tasks)
|
|
self.logger.info(f"解决了 {len(conflicts)} 个路径冲突")
|
return resolved_paths
|
|
def _resolve_single_conflict(self, paths: List[Dict], conflict: Conflict, executing_tasks: List[Dict] = None) -> List[Dict]:
|
"""
|
解决单个冲突
|
|
Args:
|
paths: 当前路径列表
|
conflict: 冲突
|
executing_tasks: 执行中的任务列表
|
|
Returns:
|
List[Dict]: 更新后的路径列表
|
"""
|
if conflict.type == "vertex":
|
return self._resolve_vertex_conflict(paths, conflict, executing_tasks)
|
elif conflict.type == "edge":
|
return self._resolve_edge_conflict(paths, conflict, executing_tasks)
|
elif conflict.type == "follow":
|
return self._resolve_following_conflict(paths, conflict, executing_tasks)
|
|
return paths
|
|
def _resolve_vertex_conflict(self, paths: List[Dict], conflict: Conflict, executing_tasks: List[Dict] = None) -> List[Dict]:
|
"""解决顶点冲突 - 基于优先级评估选择让步的AGV"""
|
updated_paths = paths.copy()
|
|
# 找到冲突的AGV路径
|
agv1_path = None
|
agv2_path = None
|
agv1_index = agv2_index = -1
|
|
for i, path in enumerate(updated_paths):
|
if path.get('agvId') == conflict.agv1:
|
agv1_path = path
|
agv1_index = i
|
elif path.get('agvId') == conflict.agv2:
|
agv2_path = path
|
agv2_index = i
|
|
if not agv1_path or not agv2_path:
|
return updated_paths
|
|
# 评估两个AGV的优先级
|
agv1_priority = self.evaluate_agv_priority(conflict.agv1, agv1_path, executing_tasks)
|
agv2_priority = self.evaluate_agv_priority(conflict.agv2, agv2_path, executing_tasks)
|
|
# 选择优先级较低的AGV进行等待
|
if agv1_priority.priority_score <= agv2_priority.priority_score:
|
waiting_agv_path = agv1_path
|
waiting_agv_index = agv1_index
|
waiting_agv_id = conflict.agv1
|
higher_priority_agv_id = conflict.agv2
|
else:
|
waiting_agv_path = agv2_path
|
waiting_agv_index = agv2_index
|
waiting_agv_id = conflict.agv2
|
higher_priority_agv_id = conflict.agv1
|
|
# 记录详细的避让决策信息
|
self.logger.info(f"顶点冲突避让决策 - 位置: {conflict.position1}, 时间: {conflict.time_step}")
|
self.logger.info(f" AGV {conflict.agv1}: {agv1_priority.explanation}")
|
self.logger.info(f" AGV {conflict.agv2}: {agv2_priority.explanation}")
|
self.logger.info(f" 决策: AGV {waiting_agv_id} 让步给 AGV {higher_priority_agv_id}")
|
|
# 在冲突位置之前插入等待步骤
|
self._insert_wait_step(waiting_agv_path, conflict.time_step)
|
updated_paths[waiting_agv_index] = waiting_agv_path
|
|
return updated_paths
|
|
def _resolve_edge_conflict(self, paths: List[Dict], conflict: Conflict, executing_tasks: List[Dict] = None) -> List[Dict]:
|
"""解决边冲突 - 基于优先级评估选择让步的AGV"""
|
updated_paths = paths.copy()
|
|
# 找到冲突的AGV路径
|
agv1_path = None
|
agv2_path = None
|
agv1_index = agv2_index = -1
|
|
for i, path in enumerate(updated_paths):
|
if path.get('agvId') == conflict.agv1:
|
agv1_path = path
|
agv1_index = i
|
elif path.get('agvId') == conflict.agv2:
|
agv2_path = path
|
agv2_index = i
|
|
if not agv1_path or not agv2_path:
|
return updated_paths
|
|
# 评估两个AGV的优先级
|
agv1_priority = self.evaluate_agv_priority(conflict.agv1, agv1_path, executing_tasks)
|
agv2_priority = self.evaluate_agv_priority(conflict.agv2, agv2_path, executing_tasks)
|
|
# 选择优先级较低的AGV进行等待
|
if agv1_priority.priority_score <= agv2_priority.priority_score:
|
waiting_agv_path = agv1_path
|
waiting_agv_index = agv1_index
|
waiting_agv_id = conflict.agv1
|
higher_priority_agv_id = conflict.agv2
|
else:
|
waiting_agv_path = agv2_path
|
waiting_agv_index = agv2_index
|
waiting_agv_id = conflict.agv2
|
higher_priority_agv_id = conflict.agv1
|
|
# 记录详细的避让决策信息
|
self.logger.info(f"边冲突避让决策 - 位置交换: {conflict.position1}<->{conflict.position2}, 时间: {conflict.time_step}")
|
self.logger.info(f" AGV {conflict.agv1}: {agv1_priority.explanation}")
|
self.logger.info(f" AGV {conflict.agv2}: {agv2_priority.explanation}")
|
self.logger.info(f" 决策: AGV {waiting_agv_id} 让步给 AGV {higher_priority_agv_id}")
|
|
# 在冲突位置之前插入等待步骤
|
self._insert_wait_step(waiting_agv_path, conflict.time_step)
|
updated_paths[waiting_agv_index] = waiting_agv_path
|
|
return updated_paths
|
|
def _resolve_following_conflict(self, paths: List[Dict], conflict: Conflict, executing_tasks: List[Dict] = None) -> List[Dict]:
|
"""解决跟随冲突 - 基于优先级评估选择让步的AGV"""
|
updated_paths = paths.copy()
|
|
# 找到冲突的AGV路径
|
agv1_path = None
|
agv2_path = None
|
agv1_index = agv2_index = -1
|
|
for i, path in enumerate(updated_paths):
|
if path.get('agvId') == conflict.agv1:
|
agv1_path = path
|
agv1_index = i
|
elif path.get('agvId') == conflict.agv2:
|
agv2_path = path
|
agv2_index = i
|
|
if not agv1_path or not agv2_path:
|
return updated_paths
|
|
# 评估两个AGV的优先级
|
agv1_priority = self.evaluate_agv_priority(conflict.agv1, agv1_path, executing_tasks)
|
agv2_priority = self.evaluate_agv_priority(conflict.agv2, agv2_path, executing_tasks)
|
|
# 选择优先级较低的AGV进行等待
|
if agv1_priority.priority_score <= agv2_priority.priority_score:
|
waiting_agv_path = agv1_path
|
waiting_agv_index = agv1_index
|
waiting_agv_id = conflict.agv1
|
higher_priority_agv_id = conflict.agv2
|
else:
|
waiting_agv_path = agv2_path
|
waiting_agv_index = agv2_index
|
waiting_agv_id = conflict.agv2
|
higher_priority_agv_id = conflict.agv1
|
|
# 记录详细的避让决策信息
|
self.logger.info(f"跟随冲突避让决策 - 距离过近, 时间: {conflict.time_step}")
|
self.logger.info(f" AGV {conflict.agv1} (位置: {conflict.position1}): {agv1_priority.explanation}")
|
self.logger.info(f" AGV {conflict.agv2} (位置: {conflict.position2}): {agv2_priority.explanation}")
|
self.logger.info(f" 决策: AGV {waiting_agv_id} 让步给 AGV {higher_priority_agv_id}")
|
|
# 在冲突位置之前插入等待步骤
|
self._insert_wait_step(waiting_agv_path, conflict.time_step)
|
updated_paths[waiting_agv_index] = waiting_agv_path
|
|
return updated_paths
|
|
def _insert_wait_step(self, path: Dict, time_step: int):
|
"""
|
在指定时间步插入等待步骤
|
|
Args:
|
path: AGV路径
|
time_step: 插入位置的时间步
|
"""
|
code_list = path.get('codeList', [])
|
|
if time_step < len(code_list) and time_step > 0:
|
# 在指定位置插入等待步骤（重复前一个位置）
|
prev_code = code_list[time_step - 1]
|
|
# 确保复制所有字段
|
if isinstance(prev_code, dict):
|
wait_code = prev_code.copy()
|
else:
|
wait_code = {
|
'code': prev_code.code,
|
'direction': prev_code.direction
|
}
|
|
code_list.insert(time_step, wait_code)
|
path['codeList'] = code_list
|
|
def validate_four_direction_movement(self, planned_paths: List[Dict]) -> List[Dict]:
|
"""
|
验证并修正路径，确保AGV只能以0、90、180、270度移动
|
|
Args:
|
planned_paths: 规划路径列表
|
|
Returns:
|
List[Dict]: 修正后的路径列表
|
"""
|
validated_paths = []
|
|
for path_data in planned_paths:
|
validated_path = self._validate_single_path_directions(path_data)
|
validated_paths.append(validated_path)
|
|
return validated_paths
|
|
def _validate_single_path_directions(self, path_data: Dict) -> Dict:
|
"""
|
验证单个路径的移动方向
|
|
Args:
|
path_data: 单个AGV路径数据
|
|
Returns:
|
Dict: 修正后的路径数据
|
"""
|
validated_path = path_data.copy()
|
code_list = path_data.get('codeList', [])
|
|
if len(code_list) < 2:
|
return validated_path
|
|
validated_code_list = []
|
|
for i in range(len(code_list)):
|
current_code = code_list[i]
|
|
if isinstance(current_code, dict):
|
position = current_code.get('code', '')
|
direction = current_code.get('direction', '90')
|
|
# 保留原有的所有字段
|
validated_code = current_code.copy()
|
else:
|
position = current_code.code
|
direction = current_code.direction
|
|
# 转换为字典格式并保留基本字段
|
validated_code = {
|
'code': position,
|
'direction': direction
|
}
|
|
# 如果不是第一个点，计算正确的方向
|
if i > 0:
|
prev_code = code_list[i - 1]
|
prev_position = prev_code.get('code', '') if isinstance(prev_code, dict) else prev_code.code
|
|
# 计算移动方向
|
correct_direction = self._calculate_movement_direction(prev_position, position)
|
if correct_direction:
|
direction = correct_direction
|
|
# 确保方向是有效的四方向之一
|
direction = self._normalize_direction(direction)
|
|
# 更新方向，保留其他所有字段
|
validated_code['direction'] = direction
|
validated_code_list.append(validated_code)
|
|
validated_path['codeList'] = validated_code_list
|
return validated_path
|
|
def _calculate_movement_direction(self, from_position: str, to_position: str) -> Optional[str]:
|
"""
|
计算从一个位置到另一个位置的移动方向
|
|
Args:
|
from_position: 起始位置码
|
to_position: 目标位置码
|
|
Returns:
|
Optional[str]: 移动方向（0, 90, 180, 270）
|
"""
|
from_coord = get_coordinate_from_path_id(from_position, self.path_mapping)
|
to_coord = get_coordinate_from_path_id(to_position, self.path_mapping)
|
|
if not from_coord or not to_coord:
|
return None
|
|
dx = to_coord[0] - from_coord[0]
|
dy = to_coord[1] - from_coord[1]
|
|
# 只允许四个方向的移动
|
if dx > 0 and dy == 0:
|
return "0" # 向东
|
elif dx < 0 and dy == 0:
|
return "180" # 向西
|
elif dx == 0 and dy > 0:
|
return "90" # 向南
|
elif dx == 0 and dy < 0:
|
return "270" # 向北
|
else:
|
# 不是标准四方向移动，返回默认方向
|
return "90"
|
|
def _normalize_direction(self, direction: str) -> str:
|
"""
|
标准化方向值，确保只有0、90、180、270
|
|
Args:
|
direction: 原始方向值
|
|
Returns:
|
str: 标准化后的方向值
|
"""
|
try:
|
angle = int(direction) % 360
|
|
# 将角度映射到最近的四方向
|
if angle <= 45 or angle > 315:
|
return "0"
|
elif 45 < angle <= 135:
|
return "90"
|
elif 135 < angle <= 225:
|
return "180"
|
else:
|
return "270"
|
except:
|
return "90" # 默认方向
|
