431 lines
15 KiB
Python
431 lines
15 KiB
Python
import logging
|
||
import os
|
||
from datetime import datetime
|
||
from enum import Enum
|
||
from dataclasses import dataclass
|
||
from typing import List, Tuple, Optional, Any
|
||
import numpy as np
|
||
import time
|
||
import matplotlib.pyplot as plt
|
||
from matplotlib.animation import FuncAnimation
|
||
import threading
|
||
from collections import deque
|
||
|
||
@dataclass
|
||
class HistoryRecord:
|
||
"""历史记录的单个条目"""
|
||
timestamp: float
|
||
state: str # 'transport', 'middle', 'about', 'colored'
|
||
rate: float
|
||
volume: float
|
||
image: np.ndarray
|
||
|
||
|
||
class History:
|
||
"""滑动窗口历史记录管理类"""
|
||
|
||
def __init__(self, max_window_size: float = 5.0, base_time = 5.0, display: bool = False):
|
||
"""
|
||
初始化历史记录管理器
|
||
|
||
Args:
|
||
max_window_size: 最大窗口时间长度（秒）
|
||
base_time: 基准时间长度（秒）
|
||
display: 是否开启实时可视化显示
|
||
"""
|
||
if base_time > max_window_size:
|
||
max_window_size = base_time+0.2
|
||
# raise ValueError("Base time must be less than or equal to max window size.")
|
||
self.records: List[HistoryRecord] = []
|
||
self.max_window_size = max_window_size
|
||
self.fulled = False
|
||
self.about_history = [] # 用于存储最近的about状态记录
|
||
self.base = None
|
||
self._base_time = base_time
|
||
self._base_cnt = 0
|
||
self.end_history:list[bool] = []
|
||
self.display = display
|
||
self.fig: Any = None
|
||
self.ax: Any = None
|
||
self.line: Any = None
|
||
self.base_line: Any = None # 基准线的引用
|
||
self.animation: Any = None
|
||
self._plot_thread: Optional[threading.Thread] = None
|
||
self._plot_running = False
|
||
|
||
if self.display:
|
||
self._setup_plot()
|
||
def add_record(self, timestamp: float, state: str, rate: float, volume: float, image: np.ndarray):
|
||
"""添加新的历史记录"""
|
||
record = HistoryRecord(timestamp, state, rate, volume, image)
|
||
self.records.append(record)
|
||
self._cleanup_old_records(timestamp)
|
||
if self.base is None and (timestamp-self._base_time)>= self.records[0].timestamp:
|
||
if self._base_cnt < 30:
|
||
self._base_cnt+=1
|
||
return
|
||
base_records = self.get_recent_records(self._base_time, timestamp)
|
||
self.base = sum([rec.rate for rec in base_records]) / len(base_records)
|
||
get_endpoint_logger().info("Base rate calculated: %.2f", self.base)
|
||
|
||
def _cleanup_old_records(self, current_time: float):
|
||
"""清理过期的历史记录"""
|
||
cutoff_time = current_time - self.max_window_size
|
||
if not self.records and self.records[0].timestamp < cutoff_time:
|
||
return False
|
||
while self.records and self.records[0].timestamp < cutoff_time:
|
||
self.records.pop(0)
|
||
return True
|
||
|
||
def get_records_in_timespan(self, start_time: float, end_time: Optional[float] = None) -> List[HistoryRecord]:
|
||
"""获取指定时间段内的记录"""
|
||
if end_time is None:
|
||
return [record for record in self.records if record.timestamp >= start_time]
|
||
else:
|
||
return [record for record in self.records
|
||
if start_time <= record.timestamp <= end_time]
|
||
|
||
def get_recent_records(self, duration: float, current_time: float) -> List[HistoryRecord]:
|
||
"""获取最近指定时间长度内的记录"""
|
||
start_time = current_time - duration
|
||
return self.get_records_in_timespan(start_time, current_time)
|
||
|
||
def get_state_ratio(self, target_state: str, records: Optional[List[HistoryRecord]] = None) -> float:
|
||
"""计算指定状态在记录中的比例"""
|
||
if records is None:
|
||
records = self.records
|
||
|
||
if not records:
|
||
return 0.0
|
||
|
||
target_count = sum(1 for record in records if record.state == target_state)
|
||
return target_count / len(records)
|
||
|
||
def get_states_by_type(self, target_state: str) -> List[float]:
|
||
"""获取所有指定状态的时间戳"""
|
||
return [record.timestamp for record in self.records if record.state == target_state]
|
||
|
||
def find_record_by_timestamp(self, target_timestamp: float) -> Optional[HistoryRecord]:
|
||
"""根据时间戳查找记录"""
|
||
for record in self.records:
|
||
if record.timestamp == target_timestamp:
|
||
return record
|
||
return None
|
||
|
||
def is_empty(self) -> bool:
|
||
"""检查历史记录是否为空"""
|
||
return len(self.records) == 0
|
||
|
||
def __len__(self) -> int:
|
||
"""返回历史记录的数量"""
|
||
return len(self.records)
|
||
|
||
def clear(self):
|
||
"""清空所有历史记录"""
|
||
self.records.clear()
|
||
if self.display:
|
||
self._stop_plot()
|
||
|
||
def _setup_plot(self):
|
||
"""设置matplotlib图表"""
|
||
plt.ion() # 交互式模式
|
||
self.fig, self.ax = plt.subplots(figsize=(10, 6))
|
||
self.ax.set_title('实时Rate值变化')
|
||
self.ax.set_xlabel('时间 (s)')
|
||
self.ax.set_ylabel('Rate值')
|
||
self.ax.grid(True, alpha=0.3)
|
||
|
||
# 初始化空线条
|
||
self.line, = self.ax.plot([], [], 'b-', linewidth=2, label='Rate')
|
||
self.base_line = None # 基准线的引用
|
||
self.ax.legend()
|
||
|
||
# 启动更新线程
|
||
self._plot_running = True
|
||
self._plot_thread = threading.Thread(target=self._update_plot_loop, daemon=True)
|
||
self._plot_thread.start()
|
||
|
||
def _update_plot_loop(self):
|
||
"""绘图更新循环"""
|
||
while self._plot_running:
|
||
try:
|
||
self._update_plot()
|
||
time.sleep(0.1) # 100ms更新间隔
|
||
except Exception as e:
|
||
get_vision_logger().error(f"Plot update error: {e}")
|
||
break
|
||
def _update_plot(self):
|
||
"""更新图表数据 - 按照matplotlib推荐的方式重构"""
|
||
if not self.records:
|
||
return
|
||
|
||
start_time = time.time()
|
||
|
||
try:
|
||
# 获取时间和rate数据
|
||
timestamps = [record.timestamp for record in self.records]
|
||
rates = [record.rate for record in self.records]
|
||
|
||
if not timestamps:
|
||
return
|
||
|
||
# 将时间戳转换为相对时间（从第一个记录开始）
|
||
start_timestamp = timestamps[0]
|
||
relative_times = [(t - start_timestamp) for t in timestamps]
|
||
|
||
# 更新主线条数据
|
||
self.line.set_data(relative_times, rates)
|
||
|
||
# 处理base线
|
||
self._update_base_line()
|
||
|
||
# 自动调整坐标轴范围
|
||
self._update_axis_limits(relative_times, rates)
|
||
|
||
# 使用推荐的绘制方式
|
||
self.ax.draw_artist(self.line)
|
||
self.fig.canvas.draw() # 使用draw_idle而不是draw
|
||
|
||
except Exception as e:
|
||
get_vision_logger().error(f"Plot update error: {e}")
|
||
finally:
|
||
# 性能监控
|
||
elapsed = time.time() - start_time
|
||
if elapsed > 0.1:
|
||
get_vision_logger().warning(f"Plot update took too long: {elapsed:.3f}s")
|
||
|
||
def _update_base_line(self):
|
||
"""更新或创建基准线"""
|
||
if self.base is not None:
|
||
if self.base_line is None:
|
||
# 首次创建基准线
|
||
self.base_line = self.ax.axhline(
|
||
y=self.base,
|
||
color='r',
|
||
linestyle='--',
|
||
alpha=0.7,
|
||
label=f'Base Rate: {self.base:.2f}'
|
||
)
|
||
# 更新图例
|
||
self.ax.legend()
|
||
else:
|
||
# 更新现有基准线的位置
|
||
self.base_line.set_ydata([self.base, self.base])
|
||
# 更新标签
|
||
self.base_line.set_label(f'Base Rate: {self.base:.2f}')
|
||
self.ax.legend()
|
||
elif self.base_line is not None:
|
||
# 如果base被重置，移除基准线
|
||
self.base_line.remove()
|
||
self.base_line = None
|
||
self.ax.legend()
|
||
|
||
def _update_axis_limits(self, relative_times, rates):
|
||
"""智能更新坐标轴范围"""
|
||
if not relative_times or not rates:
|
||
return
|
||
|
||
# X轴范围
|
||
max_time = max(relative_times)
|
||
self.ax.set_xlim(0, max_time + max(1, max_time * 0.05)) # 添加5%的边距
|
||
|
||
# Y轴范围计算
|
||
min_rate, max_rate = min(rates), max(rates)
|
||
|
||
if self.base is not None:
|
||
# 如果有基准线，确保Y轴范围合理
|
||
y_min = 0
|
||
y_max = min(max(self.base * 35, max_rate * 1.2), 100) # 限制最大值为100
|
||
else:
|
||
# 没有基准线时的默认范围
|
||
margin = (max_rate - min_rate) * 0.1 if max_rate > min_rate else 1
|
||
y_min = max(0, min_rate - margin)
|
||
y_max = max_rate + margin
|
||
|
||
self.ax.set_ylim(y_min, y_max)
|
||
def _stop_plot(self):
|
||
"""停止绘图"""
|
||
self._plot_running = False
|
||
if self._plot_thread and self._plot_thread.is_alive():
|
||
self._plot_thread.join(timeout=1.0)
|
||
if self.fig:
|
||
plt.close(self.fig)
|
||
self.fig = None
|
||
self.ax = None
|
||
self.line = None
|
||
self.base_line = None
|
||
|
||
def save_plot(self, filename: Optional[str] = None):
|
||
"""保存当前图表"""
|
||
if not self.display or not self.fig:
|
||
get_vision_logger().warning("Plot not initialized, cannot save")
|
||
return
|
||
|
||
if filename is None:
|
||
timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
|
||
filename = f"rate_plot_{timestamp}.png"
|
||
|
||
try:
|
||
self.fig.savefig(filename, dpi=300, bbox_inches='tight')
|
||
get_vision_logger().info(f"Plot saved to {filename}")
|
||
except Exception as e:
|
||
get_vision_logger().error(f"Failed to save plot: {e}")
|
||
|
||
def toggle_display(self):
|
||
"""切换显示状态"""
|
||
if self.display:
|
||
self._stop_plot()
|
||
self.display = False
|
||
else:
|
||
self.display = True
|
||
self._setup_plot()
|
||
|
||
|
||
class State:
|
||
"""滴定状态管理类"""
|
||
class Mode(Enum):
|
||
FAST = 0 # 快速模式
|
||
SLOW = 1 # 慢速模式 (middle)
|
||
ABOUT = 2 # 接近终点模式
|
||
CRAZY = 3 # CRAZY模式
|
||
# END = 3 # 终点模式
|
||
|
||
def __init__(self, bounce_time=1, end_bounce_time=5):
|
||
self.mode = self.Mode.FAST
|
||
self.bounce_time = bounce_time
|
||
self.end_bounce_time = end_bounce_time
|
||
|
||
# 状态检查标志
|
||
self.in_middle_check = False
|
||
self.in_end_check = False
|
||
self.about_check = False
|
||
self.about_first_flag = False
|
||
|
||
# 时间记录
|
||
self.middle_detected_time = None
|
||
self.end_detected_time = None
|
||
|
||
def is_fast_mode(self):
|
||
return self.mode == self.Mode.FAST
|
||
|
||
def is_slow_mode(self):
|
||
return self.mode == self.Mode.SLOW
|
||
|
||
def is_about_mode(self):
|
||
return self.mode == self.Mode.ABOUT
|
||
|
||
# def is_end_mode(self):
|
||
# return self.mode == self.Mode.END
|
||
|
||
def enter_middle_state(self, current_time):
|
||
"""进入middle状态 - 立即切换到slow模式并开始检查"""
|
||
self.mode = self.Mode.SLOW
|
||
self.in_middle_check = True
|
||
self.middle_detected_time = current_time
|
||
|
||
def enter_about_state(self, current_time):
|
||
"""从middle状态进入about状态"""
|
||
if self.mode == self.Mode.SLOW:
|
||
self.mode = self.Mode.ABOUT
|
||
|
||
def enter_end_check(self, current_time):
|
||
"""进入end检查状态"""
|
||
self.in_end_check = True
|
||
self.end_detected_time = current_time
|
||
self.mode = self.Mode.ABOUT
|
||
|
||
def exit_middle_check(self):
|
||
"""退出middle检查状态，返回fast模式"""
|
||
self.in_middle_check = False
|
||
self.middle_detected_time = None
|
||
self.mode = self.Mode.FAST
|
||
|
||
def exit_about(self):
|
||
"""about状态退出"""
|
||
self.about_check = False
|
||
self.about_first_flag = True
|
||
if self.mode == self.Mode.ABOUT:
|
||
self.mode = self.Mode.SLOW
|
||
|
||
def should_check_middle_exit(self, current_time):
|
||
"""检查是否应该进行middle退出检查"""
|
||
return (self.in_middle_check and
|
||
self.middle_detected_time is not None and
|
||
current_time - self.middle_detected_time > self.bounce_time and
|
||
(self.mode == self.Mode.SLOW))
|
||
|
||
def should_check_end_result(self, current_time):
|
||
"""检查是否应该进行end结果检查"""
|
||
return (self.in_end_check and
|
||
self.end_detected_time is not None and
|
||
current_time - self.end_detected_time > self.end_bounce_time)
|
||
|
||
def reset_end_check(self):
|
||
"""重置end检查状态"""
|
||
self.in_end_check = False
|
||
self.end_detected_time = None
|
||
|
||
def get_status_text(self):
|
||
"""获取状态显示文本"""
|
||
status = []
|
||
current_time = time.time()
|
||
if self.in_middle_check and current_time - self.middle_detected_time > self.bounce_time:
|
||
status.append("MIDCHK")
|
||
if self.in_end_check and current_time - self.end_detected_time > self.end_bounce_time:
|
||
status.append("ENDCHK")
|
||
return ", " + ", ".join(status) if status else ""
|
||
|
||
|
||
def setup_logging(log_level=logging.INFO, log_dir="logs"):
|
||
"""
|
||
设置logging配置，创建不同模块的logger
|
||
|
||
Args:
|
||
log_level: 日志级别，默认INFO
|
||
log_dir: 日志文件存储目录，默认"logs"
|
||
"""
|
||
# 创建日志目录
|
||
if not os.path.exists(log_dir):
|
||
os.makedirs(log_dir)
|
||
|
||
# 获取当前时间作为日志文件名
|
||
timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
|
||
log_file = os.path.join(log_dir, f"titration_{timestamp}.log")
|
||
|
||
# 配置根logger
|
||
logging.basicConfig(
|
||
level=log_level,
|
||
format='%(asctime)s - %(name)8s - %(levelname)7s - %(message)s',
|
||
handlers=[
|
||
logging.FileHandler(log_file, encoding='utf-8'),
|
||
logging.StreamHandler() # 同时输出到控制台
|
||
]
|
||
)
|
||
|
||
return log_file
|
||
|
||
|
||
def get_system_logger():
|
||
"""系统初始化和控制相关的logger"""
|
||
return logging.getLogger("System")
|
||
|
||
def get_hardware_logger():
|
||
"""硬件控制相关的logger（CH340等）"""
|
||
return logging.getLogger("Hardware")
|
||
|
||
def get_vision_logger():
|
||
"""图像处理和预测相关的logger"""
|
||
return logging.getLogger("Vision")
|
||
|
||
def get_control_logger():
|
||
"""控制逻辑相关的logger"""
|
||
return logging.getLogger("Control")
|
||
|
||
def get_endpoint_logger():
|
||
"""终点检测相关的logger"""
|
||
return logging.getLogger("Endpoint")
|
||
|
||
def get_volume_logger():
|
||
"""体积计算相关的logger"""
|
||
return logging.getLogger("Volume")
|