"""
|
监控RCS系统状态并生成路径
|
"""
|
import time
|
import threading
|
import logging
|
from typing import Dict, List, Optional, Any
|
import queue
|
|
from common.data_models import AGVStatus, PlannedPath, from_dict
|
from common.api_client import RCSAPIClient
|
from algorithm_system.algorithms.path_planning import BatchPathPlanner
|
from algorithm_system.models.agv_model import AGVModelManager
|
from common.utils import load_path_mapping, generate_segment_id
|
|
try:
|
from config.settings import RCS_SERVER_HOST, RCS_SERVER_PORT, MONITOR_POLLING_INTERVAL
|
except ImportError:
|
RCS_SERVER_HOST = "10.10.10.156"
|
RCS_SERVER_PORT = 8088
|
MONITOR_POLLING_INTERVAL = 5.0
|
logging.warning("无法从config.settings导入配置，使用默认值")
|
|
|
class PathMonitorService:
|
"""监控RCS状态并生成路径"""
|
|
def __init__(self,
|
rcs_host: str = None,
|
rcs_port: int = None,
|
poll_interval: float = None,
|
path_algorithm: str = "A_STAR",
|
auto_send_paths: bool = True):
|
#初始化路径监控服务
|
self.logger = logging.getLogger(__name__)
|
|
self.rcs_host = rcs_host or RCS_SERVER_HOST
|
self.rcs_port = rcs_port or RCS_SERVER_PORT
|
self.poll_interval = poll_interval or MONITOR_POLLING_INTERVAL
|
self.path_algorithm = path_algorithm
|
self.auto_send_paths = auto_send_paths
|
|
self.rcs_client = RCSAPIClient(self.rcs_host, self.rcs_port, timeout=10)
|
self.path_mapping = load_path_mapping()
|
self.agv_manager = AGVModelManager(self.path_mapping)
|
|
self.path_planner = BatchPathPlanner(
|
self.path_mapping,
|
self.path_algorithm,
|
self.agv_manager
|
)
|
|
self.is_running = False
|
self.monitor_thread = None
|
self._stop_event = threading.Event()
|
|
self.stats = {
|
'poll_count': 0, # 轮询次数
|
'successful_polls': 0, # 成功轮询次数
|
'path_generations': 0, # 路径生成次数
|
'last_poll_time': None, # 最后轮询时间
|
'last_generation_time': None # 最后生成时间
|
}
|
|
self.logger.info(f"路径监控服务初始化完成 - 轮询间隔: {poll_interval}s, 算法: {path_algorithm}")
|
|
def start_monitoring(self):
|
"""启动监控服务"""
|
if self.is_running:
|
self.logger.warning("监控服务已在运行中")
|
return
|
|
self.is_running = True
|
self._stop_event.clear()
|
|
self.monitor_thread = threading.Thread(
|
target=self._monitoring_loop,
|
name="PathMonitorThread",
|
daemon=True
|
)
|
self.monitor_thread.start()
|
|
self.logger.info("监控服务已启动")
|
|
def stop_monitoring(self):
|
"""停止监控服务"""
|
if not self.is_running:
|
self.logger.warning("监控服务未在运行")
|
return
|
|
self.is_running = False
|
self._stop_event.set()
|
|
if self.monitor_thread and self.monitor_thread.is_alive():
|
self.monitor_thread.join(timeout=5.0)
|
|
self.logger.info("监控服务已停止")
|
|
def _monitoring_loop(self):
|
"""监控主循环"""
|
self.logger.info("开始监控循环...")
|
|
while self.is_running and not self._stop_event.is_set():
|
try:
|
self._perform_monitoring_cycle()
|
|
if not self._stop_event.wait(self.poll_interval):
|
continue
|
else:
|
break
|
|
except Exception as e:
|
self.logger.error(f"监控循环异常: {e}")
|
self._stop_event.wait(min(self.poll_interval * 2, 10.0))
|
|
self.logger.info("监控循环已结束")
|
|
def _perform_monitoring_cycle(self):
|
"""执行一次监控周期"""
|
start_time = time.time()
|
|
self.stats['poll_count'] += 1
|
self.stats['last_poll_time'] = start_time
|
|
try:
|
self.logger.info(f"开始路径监控周期 #{self.stats['poll_count']}")
|
|
# 1. 获取当前AGV状态和任务状态
|
current_agv_status, current_task_status = self._fetch_rcs_status()
|
|
if not current_agv_status:
|
self.logger.debug("未获取到AGV状态数据，跳过路径生成")
|
return
|
|
# 2. 直接根据当前状态生成免碰撞路径
|
self.logger.info(f"[路径生成] 开始为 {len(current_agv_status)} 个AGV生成路径")
|
|
generated_paths = self._generate_collision_free_paths(current_agv_status)
|
|
# 3. 发送路径到RCS（根据配置决定）
|
if generated_paths:
|
if self.auto_send_paths:
|
self._send_paths_to_rcs(generated_paths)
|
else:
|
self.logger.debug(f"路径生成完成但未发送到RCS - AGV数量: {len(current_agv_status)}, 路径数: {len(generated_paths)}")
|
|
self.logger.info(f"路径生成完成 - AGV数量: {len(current_agv_status)}, 路径数: {len(generated_paths)}, 自动发送: {self.auto_send_paths}")
|
else:
|
self.logger.debug("未生成任何路径（可能所有AGV都处于空闲状态）")
|
|
# 更新成功统计
|
self.stats['successful_polls'] += 1
|
self.stats['path_generations'] += 1
|
self.stats['last_generation_time'] = time.time()
|
|
generation_time = (time.time() - start_time) * 1000
|
self.logger.debug(f"监控周期完成 - 耗时: {generation_time:.2f}ms")
|
|
except Exception as e:
|
self.logger.error(f"监控周期执行失败: {e}")
|
|
def _generate_unique_seg_id(self, agv_id: str, task_id: str = '') -> str:
|
"""生成segId"""
|
try:
|
if task_id:
|
# 有任务时使用task_id
|
return generate_segment_id(agv_id=agv_id, task_id=task_id, action_type="2")
|
else:
|
# 无任务时使用时间戳作为目标位置，确保不同时间有不同segId
|
import time
|
time_target = f"IDLE_{int(time.time() / 3600)}" # 按小时分组
|
return generate_segment_id(agv_id=agv_id, target_position=time_target, action_type="4")
|
|
except Exception as e:
|
self.logger.error(f"生成segId失败: {e}")
|
# 降级方案：使用common.utils的函数生成备用ID
|
import time
|
fallback_target = f"FALLBACK_{int(time.time())}"
|
return generate_segment_id(agv_id=agv_id, target_position=fallback_target, action_type="1")
|
|
def _fetch_rcs_status(self) -> tuple[List[AGVStatus], Dict]:
|
"""从RCS获取当前状态"""
|
agv_status_list = []
|
task_status = {}
|
|
try:
|
# 获取AGV状态 - 使用空参数获取所有AGV状态和任务状态
|
self.logger.info(" 轮询目标: 使用空参数(agvId=None, mapId=None)获取RCS所有AGV状态和任务状态")
|
agv_response = self.rcs_client.get_agv_status(agv_id=None, map_id=None)
|
|
if agv_response.code == 200 and agv_response.data:
|
self.logger.info(f"成功获取AGV状态 - 数量: {len(agv_response.data)}")
|
|
# 统计信息
|
total_tasks = 0
|
executing_tasks = 0
|
loaded_tasks = 0
|
|
self.logger.info("=" * 80)
|
self.logger.info("[详细AGV状态信息]")
|
self.logger.info("=" * 80)
|
|
for i, agv_data in enumerate(agv_response.data, 1):
|
try:
|
# 转换为AGVStatus对象
|
agv_status = from_dict(AGVStatus, agv_data)
|
agv_status_list.append(agv_status)
|
|
# 打印AGV基本信息
|
self.logger.info(f"[AGV #{i}] {agv_status.agvId}")
|
self.logger.info(f" 状态: {agv_status.status} | 位置: {agv_status.position} | 方向: {agv_status.direction}")
|
self.logger.info(f" 电压: {agv_status.vol} | 错误码: {agv_status.error} | 空背篓: {agv_status.empty}")
|
|
# 分析背篓信息
|
if agv_status.backpack:
|
self.logger.info(f" [背篓信息] ({len(agv_status.backpack)} 个):")
|
for backpack_item in agv_status.backpack:
|
status_text = "执行中" if backpack_item.execute else ("已装载" if backpack_item.loaded else "空闲")
|
task_info = f"任务: {backpack_item.taskId}" if backpack_item.taskId else "无任务"
|
|
self.logger.info(f" [位置{backpack_item.index}] {task_info} | "
|
f"状态: {status_text} | 已装载: {backpack_item.loaded} | 执行中: {backpack_item.execute}")
|
|
# 统计任务
|
if backpack_item.taskId:
|
total_tasks += 1
|
if backpack_item.execute:
|
executing_tasks += 1
|
if backpack_item.loaded:
|
loaded_tasks += 1
|
|
# 提取任务状态信息
|
task_status[backpack_item.taskId] = {
|
'agvId': agv_status.agvId,
|
'loaded': backpack_item.loaded,
|
'execute': backpack_item.execute,
|
'index': backpack_item.index
|
}
|
else:
|
self.logger.info(f" [背篓信息] 无背篓信息")
|
|
self.logger.info("-" * 60)
|
except Exception as e:
|
self.logger.warning(f"解析AGV状态数据失败: {agv_data} - {e}")
|
|
# 打印统计摘要
|
self.logger.info("[任务统计摘要]")
|
self.logger.info(f" 总AGV数量: {len(agv_status_list)}")
|
self.logger.info(f" 总任务数量: {total_tasks}")
|
self.logger.info(f" 执行中任务: {executing_tasks}")
|
self.logger.info(f" 已装载任务: {loaded_tasks}")
|
self.logger.info(f" 未装载任务: {total_tasks - loaded_tasks}")
|
self.logger.info("=" * 80)
|
|
else:
|
self.logger.warning(f"获取AGV状态失败: {agv_response.msg if agv_response else '无响应'}")
|
|
except Exception as e:
|
self.logger.error(f"从RCS获取状态失败: {e}")
|
|
return agv_status_list, task_status
|
|
|
|
def _generate_collision_free_paths(self, agv_status_list: List[AGVStatus]) -> List[Dict]:
|
"""为所有AGV生成免碰撞路径"""
|
try:
|
self.logger.debug(f"开始为 {len(agv_status_list)} 个AGV生成免碰撞路径")
|
|
# 使用批量路径规划器生成路径
|
result = self.path_planner.plan_all_agv_paths(
|
agv_status_list=agv_status_list,
|
include_idle_agv=False, # 只为执行任务的AGV生成路径
|
constraints=None
|
)
|
|
planned_paths = result.get('plannedPaths', result.get('planned_paths', []))
|
|
# 诊断：显示路径规划结果的所有字段
|
self.logger.debug(f"路径规划器返回的字段: {list(result.keys())}")
|
self.logger.debug(f"planned_paths字段存在: {'planned_paths' in result}")
|
self.logger.debug(f"plannedPaths字段存在: {'plannedPaths' in result}")
|
self.logger.debug(f"最终获取到的路径数量: {len(planned_paths)}")
|
|
if planned_paths:
|
self.logger.info(f"成功生成 {len(planned_paths)} 条免碰撞路径")
|
|
# 记录每个AGV的路径信息
|
for path_info in planned_paths:
|
agv_id = path_info.get('agvId', 'unknown')
|
code_list = path_info.get('codeList', [])
|
self.logger.debug(f"AGV {agv_id} 路径长度: {len(code_list)}")
|
else:
|
self.logger.info("未生成任何路径（可能所有AGV都处于空闲状态）")
|
|
return planned_paths
|
|
except Exception as e:
|
self.logger.error(f"生成免碰撞路径失败: {e}")
|
return []
|
|
|
|
def _send_paths_to_rcs(self, generated_paths: List[Dict]):
|
"""将生成的路径批量发送到RCS系统"""
|
try:
|
self.logger.info(f"开始批量发送 {len(generated_paths)} 条路径到RCS系统")
|
|
# 构建批量路径数据
|
agv_paths = []
|
import uuid
|
import time as time_module
|
|
for path_info in generated_paths:
|
agv_id = path_info.get('agvId')
|
code_list = path_info.get('codeList', [])
|
|
if agv_id and code_list:
|
path_task_id = ''
|
for path_code in code_list:
|
if path_code.get('taskId'):
|
path_task_id = path_code.get('taskId')
|
break
|
|
navigation_data = []
|
for path_code in code_list:
|
nav_item = {
|
'code': path_code.get('code', ''),
|
'direction': path_code.get('direction', '90'),
|
'type': path_code.get('type')
|
}
|
|
if path_code.get('taskId'):
|
nav_item['taskId'] = path_code.get('taskId')
|
|
if path_code.get('posType'):
|
nav_item['posType'] = path_code.get('posType')
|
|
if path_code.get('lev') is not None:
|
nav_item['lev'] = path_code.get('lev')
|
|
navigation_data.append(nav_item)
|
|
auto_seg_id = self._generate_unique_seg_id(agv_id, path_task_id)
|
|
agv_path_data = {
|
'agvId': agv_id,
|
'segId': auto_seg_id,
|
'codeList': navigation_data
|
}
|
|
agv_paths.append(agv_path_data)
|
|
if not agv_paths:
|
self.logger.warning("没有有效的路径数据需要发送")
|
return
|
|
try:
|
import requests
|
import json
|
|
rcs_url = f"http://{self.rcs_host}:{self.rcs_port}/api/open/algorithm/zkd/navigation/v1"
|
|
self.logger.info(f"算法系统发送路径规划结果到RCS:")
|
self.logger.info(f" AGV数量: {len(agv_paths)}")
|
self.logger.info(f" 目标URL: {rcs_url}")
|
|
successful_sends = 0
|
|
for agv_path in agv_paths:
|
agv_id = agv_path['agvId']
|
seg_id = agv_path['segId']
|
code_list = agv_path['codeList']
|
|
# 构造标准导航请求数据（agvId, segId, codeList格式）
|
navigation_data = {
|
'agvId': agv_id,
|
'segId': seg_id,
|
'codeList': code_list
|
}
|
|
# 显示路径摘要
|
start_code = code_list[0].get('code', '') if code_list else ''
|
end_code = code_list[-1].get('code', '') if code_list else ''
|
task_codes = [nc for nc in code_list if nc.get('taskId')]
|
task_id = task_codes[0].get('taskId', '') if task_codes else ''
|
|
self.logger.info(f" AGV: {agv_id}, 任务: {task_id}, 路径: {start_code} -> {end_code} ({len(code_list)}点)")
|
|
# 详细显示路径JSON数据
|
self.logger.info(f"发送路径JSON数据:")
|
self.logger.info(json.dumps(navigation_data, ensure_ascii=False, indent=2))
|
|
try:
|
response = requests.post(
|
rcs_url,
|
json=navigation_data,
|
timeout=10,
|
headers={'Content-Type': 'application/json'}
|
)
|
|
if response.status_code == 200:
|
response_data = response.json()
|
self.logger.info(f" 成功发送路径到RCS - AGV: {agv_id}, 响应: {response_data.get('msg', 'OK')}")
|
successful_sends += 1
|
else:
|
self.logger.warning(f" 发送路径到RCS失败 - AGV: {agv_id}, HTTP状态: {response.status_code}")
|
self.logger.warning(f" 响应内容: {response.text}")
|
|
except Exception as e:
|
self.logger.warning(f" 发送导航指令异常 - AGV: {agv_id}, 错误: {e}")
|
|
self.logger.info(f" 路径发送完成 - 成功: {successful_sends}/{len(agv_paths)}")
|
|
except Exception as e:
|
self.logger.error(f" 发送导航指令异常: {e}")
|
|
except Exception as e:
|
self.logger.error(f"发送路径到RCS异常: {e}")
|
|
def get_monitoring_status(self) -> Dict[str, Any]:
|
"""获取监控服务状态"""
|
return {
|
'is_running': self.is_running,
|
'rcs_host': self.rcs_host,
|
'rcs_port': self.rcs_port,
|
'poll_interval': self.poll_interval,
|
'path_algorithm': self.path_algorithm,
|
'auto_send_paths': self.auto_send_paths,
|
'stats': self.stats.copy()
|
}
|
|
def reset_stats(self):
|
"""重置统计信息"""
|
self.stats = {
|
'poll_count': 0,
|
'successful_polls': 0,
|
'path_generations': 0,
|
'last_poll_time': None,
|
'last_generation_time': None
|
}
|
self.logger.info("监控统计信息已重置")
|
