grpc_client/clients/dexhand_client.py

503 lines
18 KiB
Python
Raw Normal View History

2026-03-18 09:19:23 +08:00
import time
import threading
from typing import Optional, List
import numpy as np
import grpc
from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout,
QHBoxLayout, QLabel, QPushButton, QComboBox,
QSpinBox, QGroupBox, QGridLayout, QLineEdit, QMessageBox)
from PyQt5.QtCore import QTimer, Qt
from PyQt5.QtGui import QFont
import pyqtgraph as pg
from clients._path_setup import ensure_paths
ensure_paths()
from cmvr.api import dexhand_service_pb2_grpc
from cmvr.api import dexhand_command_pb2
from google.protobuf.timestamp_pb2 import Timestamp
class DexHandSensorClient:
"""灵巧手传感器数据客户端"""
def __init__(self, ip="192.168.0.222", port=50052, device_id="dexhand_001", timeout=2, retries=1):
self.ip = ip
self.port = port
self.address = f"{ip}:{port}"
self.device_id = device_id
self.timeout = timeout
self.retries = retries
self.channel = None
self.stub = None
self._connect_with_retry()
def _connect_with_retry(self):
"""连接gRPC服务器"""
attempt = 0
while attempt <= self.retries:
print(f"Connecting to DexHand gRPC server at {self.address}... (Attempt {attempt + 1})")
try:
self.channel = grpc.insecure_channel(self.address)
grpc.channel_ready_future(self.channel).result(timeout=self.timeout)
self.stub = dexhand_service_pb2_grpc.DexHandServiceStub(self.channel)
print(f"Successfully connected to DexHand gRPC server: {self.address}")
print(f"Device ID: {self.device_id}")
return True
except grpc.FutureTimeoutError:
attempt += 1
print(f"Connection timed out, attempt {attempt} failed")
if attempt > self.retries:
return False
print(f"Waiting {self.timeout}s before retrying...")
time.sleep(self.timeout)
except Exception as e:
print(f"Connection error: {e}")
return False
return False
def get_sensor_data(self) -> Optional[dexhand_command_pb2.GetSensorDataCommand.Feedback]:
"""获取传感器数据"""
if not self.stub:
print("No gRPC connection available")
return None
try:
# 创建请求
timestamp = Timestamp()
timestamp.GetCurrentTime()
request = dexhand_command_pb2.GetSensorDataCommand.Request()
request.header.device_id = self.device_id
request.header.timestamp.CopyFrom(timestamp)
# 发送请求
feedback = self.stub.GetSensorData(request)
return feedback
except grpc.RpcError as e:
print(f"gRPC error: {e.code()} - {e.details()}")
return None
except Exception as e:
print(f"Error getting sensor data: {e}")
return None
def close(self):
"""关闭连接"""
if self.channel:
self.channel.close()
print(f"Closed connection to DexHand gRPC server {self.address}")
def is_connected(self):
"""检查是否已连接"""
return self.channel is not None and self.stub is not None
class SensorDataPlotWidget(QWidget):
"""传感器数据绘制窗口"""
def __init__(self, parent=None):
super().__init__(parent)
self.client = None
self.update_timer = QTimer()
self.data_buffer = {
'sum': [], # sum数据缓冲区
'peak': [] # peak数据缓冲区
}
self.time_buffer = [] # 时间戳缓冲区
self.max_points = 100 # 最大显示点数
self.current_finger = dexhand_command_pb2.SensorData.INDEX # 默认食指
self.current_part = dexhand_command_pb2.SensorData.TIP # 默认指端
self.setup_ui()
self.setup_plots()
def setup_ui(self):
"""设置UI"""
layout = QVBoxLayout()
self.setLayout(layout)
# 标题
title = QLabel("灵巧手传感器数据监控 - 食指指端")
title_font = QFont()
title_font.setPointSize(14)
title_font.setBold(True)
title.setFont(title_font)
title.setAlignment(Qt.AlignCenter)
layout.addWidget(title)
# 控制面板
control_group = QGroupBox("连接配置")
control_layout = QGridLayout()
control_group.setLayout(control_layout)
# IP地址
control_layout.addWidget(QLabel("服务器IP:"), 0, 0)
self.ip_edit = QLineEdit("192.168.0.222")
control_layout.addWidget(self.ip_edit, 0, 1)
# 端口
control_layout.addWidget(QLabel("端口:"), 1, 0)
self.port_edit = QLineEdit("50052")
control_layout.addWidget(self.port_edit, 1, 1)
# 设备ID
control_layout.addWidget(QLabel("设备ID:"), 2, 0)
self.device_id_edit = QLineEdit("dexhand_001")
control_layout.addWidget(self.device_id_edit, 2, 1)
# 完整地址显示
control_layout.addWidget(QLabel("完整地址:"), 3, 0)
self.address_label = QLabel("192.168.0.222:50052")
self.address_label.setStyleSheet("font-weight: bold;")
control_layout.addWidget(self.address_label, 3, 1)
# 连接按钮
self.connect_btn = QPushButton("连接")
self.connect_btn.clicked.connect(self.toggle_connection)
self.connect_btn.setMinimumHeight(40)
self.connect_btn.setStyleSheet("font-weight: bold;")
control_layout.addWidget(self.connect_btn, 4, 0, 1, 2)
# 连接状态
self.status_label = QLabel("未连接")
self.status_label.setAlignment(Qt.AlignCenter)
self.status_label.setStyleSheet("color: red; font-weight: bold; padding: 5px;")
control_layout.addWidget(self.status_label, 5, 0, 1, 2)
layout.addWidget(control_group)
# 数据配置面板
data_config_group = QGroupBox("数据配置")
data_config_layout = QGridLayout()
data_config_group.setLayout(data_config_layout)
# 更新频率
data_config_layout.addWidget(QLabel("更新频率(ms):"), 0, 0)
self.freq_spin = QSpinBox()
self.freq_spin.setRange(10, 1000)
self.freq_spin.setValue(50)
self.freq_spin.valueChanged.connect(self.set_update_interval)
data_config_layout.addWidget(self.freq_spin, 0, 1)
# 显示点数
data_config_layout.addWidget(QLabel("显示点数:"), 1, 0)
self.points_spin = QSpinBox()
self.points_spin.setRange(10, 500)
self.points_spin.setValue(100)
self.points_spin.valueChanged.connect(self.set_max_points)
data_config_layout.addWidget(self.points_spin, 1, 1)
# 清空数据按钮
self.clear_btn = QPushButton("清空数据")
self.clear_btn.clicked.connect(self.clear_data)
self.clear_btn.setEnabled(False)
data_config_layout.addWidget(self.clear_btn, 2, 0)
# 测试连接按钮
self.test_btn = QPushButton("测试连接")
self.test_btn.clicked.connect(self.test_connection)
data_config_layout.addWidget(self.test_btn, 2, 1)
layout.addWidget(data_config_group)
# 创建绘图区域
plots_widget = QWidget()
plots_layout = QVBoxLayout()
plots_widget.setLayout(plots_layout)
# SUM曲线图
self.sum_plot_widget = pg.PlotWidget(title="食指指端传感器数据 - SUM值")
self.sum_plot_widget.setLabel('left', 'SUM值')
self.sum_plot_widget.setLabel('bottom', '时间 (秒)')
self.sum_plot_widget.showGrid(x=True, y=True, alpha=0.3)
self.sum_plot_widget.addLegend()
self.sum_curve = self.sum_plot_widget.plot(pen='y', name='SUM值', width=2)
plots_layout.addWidget(self.sum_plot_widget)
# PEAK曲线图
self.peak_plot_widget = pg.PlotWidget(title="食指指端传感器数据 - PEAK值")
self.peak_plot_widget.setLabel('left', 'PEAK值')
self.peak_plot_widget.setLabel('bottom', '时间 (秒)')
self.peak_plot_widget.showGrid(x=True, y=True, alpha=0.3)
self.peak_plot_widget.addLegend()
self.peak_curve = self.peak_plot_widget.plot(pen='r', name='PEAK值', width=2)
plots_layout.addWidget(self.peak_plot_widget)
layout.addWidget(plots_widget)
# 实时数据展示
data_group = QGroupBox("当前数据")
data_layout = QHBoxLayout()
data_group.setLayout(data_layout)
self.sum_label = QLabel("SUM: --")
self.sum_label.setStyleSheet("font-weight: bold; color: orange; font-size: 12pt;")
data_layout.addWidget(self.sum_label)
self.peak_label = QLabel("PEAK: --")
self.peak_label.setStyleSheet("font-weight: bold; color: red; font-size: 12pt;")
data_layout.addWidget(self.peak_label)
self.timestamp_label = QLabel("时间: --")
self.timestamp_label.setStyleSheet("font-size: 10pt;")
data_layout.addWidget(self.timestamp_label)
layout.addWidget(data_group)
# 连接输入变化信号
self.ip_edit.textChanged.connect(self.update_address_label)
self.port_edit.textChanged.connect(self.update_address_label)
def update_address_label(self):
"""更新地址标签"""
ip = self.ip_edit.text().strip()
port = self.port_edit.text().strip()
if ip and port:
self.address_label.setText(f"{ip}:{port}")
else:
self.address_label.setText("--")
def setup_plots(self):
"""初始化绘图"""
self.sum_curve.setData([], [])
self.peak_curve.setData([], [])
def test_connection(self):
"""测试连接"""
ip = self.ip_edit.text().strip()
port = self.port_edit.text().strip()
device_id = self.device_id_edit.text().strip()
if not ip or not port:
QMessageBox.warning(self, "警告", "请输入IP地址和端口")
return
try:
port = int(port)
except ValueError:
QMessageBox.warning(self, "警告", "端口必须是数字")
return
# 创建临时客户端测试连接
test_client = DexHandSensorClient(ip, port, device_id, timeout=2, retries=0)
if test_client.is_connected():
QMessageBox.information(self, "成功", "连接测试成功!")
test_client.close()
else:
QMessageBox.critical(self, "失败", "连接测试失败,请检查服务器地址和网络")
def set_client(self, client: DexHandSensorClient):
"""设置客户端"""
self.client = client
self.status_label.setText(f"已连接 (设备: {client.device_id})")
self.status_label.setStyleSheet("color: green; font-weight: bold; padding: 5px;")
self.connect_btn.setText("断开连接")
self.clear_btn.setEnabled(True)
self.ip_edit.setEnabled(False)
self.port_edit.setEnabled(False)
self.device_id_edit.setEnabled(False)
self.test_btn.setEnabled(False)
def clear_client(self):
"""清除客户端"""
self.client = None
self.status_label.setText("未连接")
self.status_label.setStyleSheet("color: red; font-weight: bold; padding: 5px;")
self.connect_btn.setText("连接")
self.clear_btn.setEnabled(False)
self.ip_edit.setEnabled(True)
self.port_edit.setEnabled(True)
self.device_id_edit.setEnabled(True)
self.test_btn.setEnabled(True)
def toggle_connection(self):
"""切换连接状态"""
if self.client is None or not self.client.is_connected():
# 尝试连接
ip = self.ip_edit.text().strip()
port = self.port_edit.text().strip()
device_id = self.device_id_edit.text().strip()
if not ip or not port:
QMessageBox.warning(self, "警告", "请输入IP地址和端口")
return
try:
port = int(port)
except ValueError:
QMessageBox.warning(self, "警告", "端口必须是数字")
return
# 显示连接中状态
self.status_label.setText("连接中...")
self.status_label.setStyleSheet("color: orange; font-weight: bold; padding: 5px;")
QApplication.processEvents()
# 创建客户端
try:
client = DexHandSensorClient(ip, port, device_id)
if client.is_connected():
self.set_client(client)
self.start_updating()
else:
self.clear_client()
QMessageBox.critical(self, "错误", f"无法连接到服务器 {ip}:{port}")
except Exception as e:
self.clear_client()
QMessageBox.critical(self, "错误", f"连接失败: {str(e)}")
else:
# 断开连接
self.stop_updating()
self.client.close()
self.clear_client()
self.clear_data()
def start_updating(self):
"""开始更新数据"""
self.update_timer.timeout.connect(self.update_data)
self.update_timer.start(self.freq_spin.value())
def stop_updating(self):
"""停止更新数据"""
self.update_timer.stop()
try:
self.update_timer.timeout.disconnect()
except:
pass
def set_update_interval(self, ms: int):
"""设置更新间隔"""
if self.update_timer.isActive():
self.update_timer.setInterval(ms)
def set_max_points(self, points: int):
"""设置最大显示点数"""
self.max_points = points
# 如果当前数据超过新限制,截断数据
if len(self.time_buffer) > self.max_points:
self.time_buffer = self.time_buffer[-self.max_points:]
self.data_buffer['sum'] = self.data_buffer['sum'][-self.max_points:]
self.data_buffer['peak'] = self.data_buffer['peak'][-self.max_points:]
def clear_data(self):
"""清空数据"""
self.data_buffer['sum'].clear()
self.data_buffer['peak'].clear()
self.time_buffer.clear()
self.sum_curve.setData([], [])
self.peak_curve.setData([], [])
self.sum_label.setText("SUM: --")
self.peak_label.setText("PEAK: --")
def extract_finger_data(self, feedback):
"""提取食指指端的传感器数据"""
if not feedback or not feedback.sensor:
return None, None
sum_values = []
peak_values = []
for sensor in feedback.sensor:
# 检查是否为食指指端
if (sensor.finger_type == self.current_finger and
sensor.part_type == self.current_part):
# 处理传感器数据
if sensor.data:
for row in sensor.data:
if row.values:
row_array = np.array(row.values)
sum_values.append(np.sum(row_array))
peak_values.append(np.max(row_array))
if sum_values and peak_values:
return np.mean(sum_values), np.mean(peak_values)
return None, None
def update_data(self):
"""更新数据"""
if not self.client or not self.client.is_connected():
return
feedback = self.client.get_sensor_data()
if not feedback:
# 如果获取数据失败,可能连接已断开
if self.client and not self.client.is_connected():
self.toggle_connection() # 自动断开
return
# 提取食指指端数据
sum_val, peak_val = self.extract_finger_data(feedback)
if sum_val is not None and peak_val is not None:
# 更新时间戳
current_time = time.time()
self.time_buffer.append(current_time)
self.data_buffer['sum'].append(sum_val)
self.data_buffer['peak'].append(peak_val)
# 限制缓冲区大小
if len(self.time_buffer) > self.max_points:
self.time_buffer.pop(0)
self.data_buffer['sum'].pop(0)
self.data_buffer['peak'].pop(0)
# 更新曲线
if len(self.time_buffer) > 1:
# 归一化时间显示(相对于第一个点)
t0 = self.time_buffer[0]
plot_time = [t - t0 for t in self.time_buffer]
self.sum_curve.setData(plot_time, self.data_buffer['sum'])
self.peak_curve.setData(plot_time, self.data_buffer['peak'])
# 自动调整Y轴范围
self.sum_plot_widget.autoRange()
self.peak_plot_widget.autoRange()
# 更新标签
self.sum_label.setText(f"SUM: {sum_val:.2f}")
self.peak_label.setText(f"PEAK: {peak_val:.2f}")
self.timestamp_label.setText(f"时间: {time.strftime('%H:%M:%S')}")
class SensorDataWindow(QMainWindow):
"""传感器数据监控主窗口"""
def __init__(self):
super().__init__()
self.setWindowTitle("灵巧手传感器数据监控 - 可配置版")
self.setGeometry(100, 100, 900, 1000)
# 设置中央部件
central_widget = SensorDataPlotWidget()
self.setCentralWidget(central_widget)
def closeEvent(self, event):
"""关闭窗口时清理资源"""
central_widget = self.centralWidget()
if central_widget:
central_widget.stop_updating()
if central_widget.client:
central_widget.client.close()
event.accept()
def main():
"""主函数"""
import sys
app = QApplication(sys.argv)
app.setStyle('Fusion') # 使用Fusion风格看起来更现代
window = SensorDataWindow()
window.show()
sys.exit(app.exec_())
if __name__ == "__main__":
main()