grpc_client/clients/servoj_client.py

import math
import time
import sys
from collections import deque
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np
import matplotlib.font_manager as fm

# 设置中文字体
plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'WenQuanYi Micro Hei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

from clients._path_setup import ensure_paths

ensure_paths()

from google.protobuf import timestamp_pb2

from clients.base_client import RobotClientBase

from cmvr.api import common_pb2
from cmvr.api import arm_command_pb2 as pb
from cmvr.api import arm_service_pb2_grpc as rpc


class ServoJClient(RobotClientBase):
    """Client to send ServoJ commands to the robot"""

    def send(self, joint_list, vel=1.0, device_id="right_arm"):
        """
        joint_list: list of dicts, e.g.,
            [
                {"joint_name": "L_SHOULDER_P", "rad": 0.0},
                ...
            ]
        """
        target = pb.JointPositionCommand(position=[j["rad"] for j in joint_list])

        req = pb.ServoJ.Request(
            header=self._create_header(device_id),
            target=target,
        )

        try:
            start_time = time.time()
            resp = self.stub.servoJ(req, timeout=10)
            rpc_time = time.time() - start_time
            success = getattr(resp.header, "success", None)
            error_msg = getattr(resp.header, "error_message", "")
            return success, error_msg, rpc_time
        except Exception as e:
            return False, str(e), 0.0

    def _create_header(self, device_id):
        header = common_pb2.CommandHeader.Request()
        header.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        header.timestamp.CopyFrom(ts)
        return header


def deg2rad(degrees):
    return degrees * math.pi / 180.0


class DelayPlotter:
    """实时延迟波形绘制器 - 优化版，减少闪烁"""

    def __init__(self, max_points=300):
        self.max_points = max_points
        self.rpc_delays = deque(maxlen=max_points)
        self.running = True
        self.last_update = time.time()
        self.update_interval = 0.2  # 每200ms更新一次图表，减少重绘频率

        # 创建图形 - 使用双缓冲减少闪烁
        plt.ioff()  # 关闭交互模式
        self.fig, self.ax = plt.subplots(figsize=(12, 6))
        self.fig.suptitle('gRPC Call Delay Monitor', fontsize=14)

        # RPC延迟图
        self.ax.set_ylabel('Delay (ms)')
        self.ax.set_xlabel('Sample Point')
        self.ax.set_title('gRPC Call Delay Over Time')
        self.ax.grid(True, alpha=0.3)
        self.line, = self.ax.plot([], [], 'b-', linewidth=1)
        self.ax.axhline(y=5.0, color='r', linestyle='--', alpha=0.5)

        # 设置固定的Y轴范围，避免自动缩放导致的闪烁
        self.ax.set_ylim(0, 20)

        # 设置固定的X轴范围
        self.ax.set_xlim(0, max_points)

        plt.tight_layout()

        # 启用双缓冲
        self.fig.set_dpi(100)

    def add_data(self, rpc_time):
        """添加新数据点"""
        self.rpc_delays.append(rpc_time * 1000)  # 转换为ms

    def update_plot(self):
        """手动更新图表，不使用动画"""
        if not self.running or len(self.rpc_delays) < 2:
            return

        # 控制更新频率
        current_time = time.time()
        if current_time - self.last_update < self.update_interval:
            return
        self.last_update = current_time

        x_data = list(range(len(self.rpc_delays)))
        y_data = list(self.rpc_delays)

        # 更新数据
        self.line.set_data(x_data, y_data)

        # 更新X轴范围，但保持平滑
        if len(x_data) > 0:
            self.ax.set_xlim(max(0, len(x_data) - self.max_points), len(x_data))

        # 重绘
        self.fig.canvas.draw_idle()  # 使用draw_idle代替draw，减少阻塞
        self.fig.canvas.flush_events()

    def start(self):
        """启动绘图"""
        plt.ion()  # 开启交互模式
        plt.show(block=False)

    def stop(self):
        """停止绘图"""
        self.running = False
        plt.close(self.fig)


if __name__ == "__main__":
    # 初始化客户端
    client = ServoJClient()
    joint_name = 'R_WRIST_R'

    # 固定发送0度位置
    fixed_angle = 0

    # 发送频率
    freq = 200
    dt = 1.0 / freq  # 5ms

    print(f"=== gRPC Delay Test ===")
    print(f"Joint: {joint_name}")
    print(f"Command: fixed angle {fixed_angle}°")
    print(f"Send frequency: {freq}Hz (interval {dt * 1000:.1f}ms)")
    print("-" * 50)
    print("Press Ctrl+C to stop")
    print("-" * 50)

    # 初始化绘图器
    plotter = DelayPlotter(max_points=300)
    plotter.start()

    # 统计
    sent_count = 0
    rpc_times = deque(maxlen=100)
    start_time = time.time()

    # 用于定期打印统计信息
    last_stats_time = time.time()
    stats_interval = 2.0  # 每2秒打印一次统计

    # 用于控制显示更新的频率
    last_display_time = time.time()
    display_interval = 0.05  # 每50ms更新一次控制台显示

    try:
        while True:  # 无限循环
            loop_start = time.time()

            # 发送固定角度的命令
            joint_cmd = [{'joint_name': joint_name, 'rad': deg2rad(fixed_angle)}]
            success, error, rpc_time = client.send(joint_cmd, vel=0.5)

            # 记录
            rpc_times.append(rpc_time)
            sent_count += 1

            # 添加到绘图器
            plotter.add_data(rpc_time)

            # 更新图表（受update_plot内部的频率控制）
            plotter.update_plot()

            # 控制控制台显示更新频率
            current_time = time.time()
            if current_time - last_display_time >= display_interval:
                elapsed = current_time - start_time
                avg_rpc = sum(rpc_times) / len(rpc_times) * 1000
                current_rpc = rpc_time * 1000

                sys.stdout.write(
                    f"\rTime: {elapsed:5.1f}s | "
                    f"Sent: {sent_count:6d} | "
                    f"Current: {current_rpc:6.2f}ms | "
                    f"Avg: {avg_rpc:6.2f}ms   "
                )
                sys.stdout.flush()
                last_display_time = current_time

            # 定期打印详细统计信息（每2秒）
            if current_time - last_stats_time >= stats_interval:
                if rpc_times:
                    delays_ms = [t * 1000 for t in rpc_times]
                    print(f"\n[Stats @ {current_time - start_time:.1f}s] "
                          f"Avg:{np.mean(delays_ms):.2f}ms "
                          f"Max:{np.max(delays_ms):.2f}ms "
                          f"Min:{np.min(delays_ms):.2f}ms "
                          f"P95:{np.percentile(delays_ms, 95):.2f}ms")
                last_stats_time = current_time

            # 控制发送频率
            elapsed = time.time() - loop_start
            if elapsed < dt:
                time.sleep(dt - elapsed)

    except KeyboardInterrupt:
        print("\n\n⏹️ Test stopped by user")

        # 最终统计
        total_time = time.time() - start_time
        print(f"\n=== Final Statistics ===")
        print(f"Test duration: {total_time:.2f}s")
        print(f"Total commands: {sent_count}")
        print(f"Actual frequency: {sent_count / total_time:.1f}Hz")
        if rpc_times:
            delays_ms = [t * 1000 for t in rpc_times]
            print(f"Average RPC: {np.mean(delays_ms):.2f}ms")
            print(f"Maximum RPC: {np.max(delays_ms):.2f}ms")
            print(f"Minimum RPC: {np.min(delays_ms):.2f}ms")
            print(f"StdDev RPC: {np.std(delays_ms):.2f}ms")
            print(f"P95 RPC: {np.percentile(delays_ms, 95):.2f}ms")
            print(f"P99 RPC: {np.percentile(delays_ms, 99):.2f}ms")

        # 等待用户查看图表
        print("\nClose the graph window to exit...")
        try:
            input("Press Enter to close...")  # 等待用户输入再关闭
        except:
            pass

    finally:
        plotter.stop()
        client.close()
        print("Client closed")