cmvr-es-cli/example_usage.py

#!/usr/bin/env python3

import sys
import os
import time
import random
import grpc  # 新增grpc导入
import cv2
from typing import Generator  # 新增Generator类型导入
import numpy as np  # 确保导入numpy（传感器数据处理需要）

# 添加项目根目录到 Python 路径
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

# 直接导入生成的模块
from generated.cmvr.api import biohead_command_pb2
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState,JointCmd, Pose3D,RobotCartesian, RobotJointIndexDirection

def test_biohead():
    """测试仿生头功能"""
    print("=== 测试仿生头功能 ===")

    # 初始化客户端
    client = CMVRGrpcClient("192.168.1.222:50051")
    if not client.is_connected():
        print("连接服务器失败")
        return

    # 获取仿生头
    biohead = client.get_biohead("bio_head")

    # 创建表情
    expression = FacialExpressionState()
    expression.left_eyebrow_outside_y = random.uniform(0, 1)
    expression.jaw_x = 0.5
    expression.jaw_y = 0.5
    # # 设置眉毛，使用随机数并打印
    # expression.left_eyebrow_outside_y = random.uniform(0, 1)
    # expression.left_eyebrow_inside_y = random.uniform(0, 1)
    # expression.right_eyebrow_outside_y = random.uniform(0, 1)
    # expression.right_eyebrow_inside_y = random.uniform(0, 1)
    # print(f"眉毛设置: 左外 {expression.left_eyebrow_outside_y}, 左内 {expression.left_eyebrow_inside_y}, 右外 {expression.right_eyebrow_outside_y}, 右内 {expression.right_eyebrow_inside_y}")
    #
    # # 设置眼睑，使用随机数并打印
    # expression.left_eye_upper_lid_y = random.uniform(0, 1)
    # expression.left_eye_lower_lid_y = random.uniform(0, 1)
    # expression.right_eye_upper_lid_y = random.uniform(0, 1)
    # expression.right_eye_lower_lid_y = random.uniform(0, 1)
    # print(f"眼睑设置: 左上 {expression.left_eye_upper_lid_y}, 左下 {expression.left_eye_lower_lid_y}, 右上 {expression.right_eye_upper_lid_y}, 右下 {expression.right_eye_lower_lid_y}")
    #
    # # 设置眼球，使用随机数并打印
    # expression.left_eye_ball_x = random.uniform(0, 1)
    # expression.left_eye_ball_y = random.uniform(0, 1)
    # expression.right_eye_ball_x = random.uniform(0, 1)
    # expression.right_eye_ball_y = random.uniform(0, 1)
    # print(f"眼球设置: 左X {expression.left_eye_ball_x}, 左Y {expression.left_eye_ball_y}, 右X {expression.right_eye_ball_x}, 右Y {expression.right_eye_ball_y}")
    #
    # # 设置嘴巴，使用随机数并打印
    # expression.upper_lip_y = random.uniform(0, 1)
    # expression.lower_lip_y = random.uniform(0, 1)
    # print(f"嘴巴设置: 上唇 {expression.upper_lip_y}, 下唇 {expression.lower_lip_y}")
    #
    # # 设置下巴，使用随机数并打印
    # expression.jaw_x = random.uniform(0, 1)
    # expression.jaw_y = random.uniform(0, 1)
    # print(f"下巴设置: X {expression.jaw_x}, Y {expression.jaw_y}")

    # 设置表情
    result = biohead.set_expression(expression)
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("设置表情成功")
    else:
        print(f"设置表情失败，错误码: {result}")

    # 测试流式控制
    print("开始流式控制表情 (5次眨眼)...")
    result = biohead.start_stream()
    if result != CMVRErrorCode.CMVR_SUCCESS:
        print(f"开始流式会话失败，错误码: {result}")
    else:
        for i in range(5):
            # 创建 protobuf 消息用于流式传输
            proto_expr = biohead_command_pb2.FacialExpression()

            # 睁眼，使用随机数并打印
            proto_expr.eyelid.left_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.left_lower_y = random.uniform(0, 1)
            proto_expr.eyelid.right_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.right_lower_y = random.uniform(0, 1)
            print(f"第{i+1}次流式睁眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}")

            result = biohead.stream_expression(proto_expr)
            if result == CMVRErrorCode.CMVR_SUCCESS:
                print(f"第{i+1}次流式睁眼成功")
            else:
                print(f"第{i+1}次流式睁眼失败，错误码: {result}")



            # 闭眼，使用随机数并打印
            proto_expr.eyelid.left_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.left_lower_y = random.uniform(0, 1)
            proto_expr.eyelid.right_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.right_lower_y = random.uniform(0, 1)
            print(f"第{i+1}次流式闭眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}")

            result = biohead.stream_expression(proto_expr)
            if result == CMVRErrorCode.CMVR_SUCCESS:
                print(f"第{i+1}次流式闭眼成功")
            else:
                print(f"第{i+1}次流式闭眼失败，错误码: {result}")



        # 结束流式会话
        result = biohead.end_stream()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("结束流式会话成功")
        else:
            print(f"结束流式会话失败，错误码: {result}")

        print("流式控制完成")

    # 获取系统状态
    result = biohead.get_system_status()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("获取系统状态成功")
    else:
        print(f"获取系统状态失败，错误码: {result}")

    # 测试紧急停止
    result = biohead.emergency_stop()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("紧急停止成功")
    else:
        print(f"紧急停止失败，错误码: {result}")

    client.close()

def test_camera():
    """测试相机功能"""
    print("=== 测试相机功能 ===")

    # 初始化客户端
    client = CMVRGrpcClient("192.168.1.222:50060")
    if not client.is_connected():
        print("连接服务器失败")
        return

    # 获取相机客户端
    camera = client.get_camera("cam4")  # 假设设备ID为"cam4"

    # 获取相机初始状态
    error_code, state = camera.get_status()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print(f"相机初始状态: 已初始化={state.is_initialized}, 已打开={state.is_opened}, "
              f"已流传输={state.is_streaming}, 已录制={state.is_recording}, "
              f"分辨率={state.width}x{state.height}, FPS={state.fps}")
    else:
        print(f"获取相机状态失败，错误码: {error_code}")
        client.close()
        return

    # 启动相机
    start_result = camera.start_camera()
    if start_result == CMVRErrorCode.CMVR_SUCCESS:
        print("相机启动成功")
    else:
        print(f"相机启动失败，错误码: {start_result}")
        client.close()
        return

    # 等待相机启动
    time.sleep(1)

    # 再次获取相机状态确认
    error_code, state = camera.get_status()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print(f"启动后状态: 已打开={state.is_opened}, 已流传输={state.is_streaming}")
    else:
        print(f"获取相机状态失败，错误码: {error_code}")

    # 测试获取RGB图像并保存
    print("开始获取图像并保存 (3次)...")
    for i in range(3):
        error_code, img_array, width, height = camera.get_rgb_image()
        if error_code == CMVRErrorCode.CMVR_SUCCESS and img_array.size > 0:
            # 生成保存路径（当前目录）
            img_filename = f"camera_test_image_{i+1}_{int(time.time())}.jpg"
            # 转换颜色格式（如果需要）
            if len(img_array.shape) == 3:
                cv2_img = cv2.cvtColor(img_array, cv2.COLOR_RGB2BGR)
            else:
                cv2_img = img_array
            # 保存图像
            cv2.imwrite(img_filename, cv2_img)
            print(f"第{i+1}次获取图像成功，已保存至: {img_filename}，分辨率: {width}x{height}")
        else:
            print(f"第{i+1}次获取图像失败，错误码: {error_code}")
        time.sleep(1)

    # 测试录像功能
    video_path = f"test_recording_{int(time.time())}.mp4"
    print(f"开始录像，保存路径: {os.path.abspath(video_path)}")  # 显示绝对路径
    record_start_result = camera.start_record(video_path)
    if record_start_result == CMVRErrorCode.CMVR_SUCCESS:
        print("录像开始成功，录制5秒...")
        time.sleep(5)

        # 停止录像
        record_stop_result = camera.stop_record()
        if record_stop_result == CMVRErrorCode.CMVR_SUCCESS:
            print("录像停止成功")
        else:
            print(f"录像停止失败，错误码: {record_stop_result}")
    else:
        print(f"录像开始失败，错误码: {record_start_result}")

    # 停止相机
    stop_result = camera.stop_camera()
    if stop_result == CMVRErrorCode.CMVR_SUCCESS:
        print("相机停止成功")
    else:
        print(f"相机停止失败，错误码: {stop_result}")

    # 最终状态确认
    error_code, state = camera.get_status()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print(f"最终状态: 已打开={state.is_opened}, 已流传输={state.is_streaming}")
    else:
        print(f"获取相机状态失败，错误码: {error_code}")

    client.close()

def test_rgb_image_stream():
    """测试双向流视频帧数据接口"""
    print("=== 测试双向流视频帧数据接口 ===")

    # 初始化GRPC客户端
    client = CMVRGrpcClient("192.168.1.222:50060")
    if not client.is_connected():
        print("连接服务器失败")
        return

    # 获取camera客户端
    camera = client.get_camera("cam4")
    if not camera:
        print("获取camera客户端失败")
        client.close()
        return
    # 启动相机
    start_result = camera.start_camera()
    if start_result == CMVRErrorCode.CMVR_SUCCESS:
        print("相机启动成功")
    else:
        print(f"相机启动失败，错误码: {start_result}")
        client.close()
        return
    # 等待相机启动
    time.sleep(1)
    try:
        # 1. 启动传感器数据流
        print("\n--- 启动rgb数据流 ---")
        # 创建请求生成器（双向流不需要参数，发送空请求）
        def request_generator() -> Generator:
            # 发送初始请求启动流
            yield camera.create_rgb_stream_request()
            # 保持流连接，每5秒发送一次心跳（可选）
            try:
                while True:
                    time.sleep(5)
                    # 发送空请求保持连接
                    yield camera.create_rgb_stream_request()
            except GeneratorExit:
                print("请求生成器已关闭")
                return

        # 2. 接收并处理流数据
        print("开始接收rgb流数据（5秒后自动停止）...")
        start_time = time.time()
        stream_duration = 15  # 接收15秒数据

        keyframe_count = 0  # 关键帧计数器
        total_frame_count = 0  # 总帧数计数器
        # 存储流迭代器的变量
        stream_iterator = camera.get_rgb_stream(request_generator())
        # 调用双向流接口
        for feedback in stream_iterator:
            # 检查是否超时
            if time.time() - start_time > stream_duration:
                print("\n达到预设接收时间，停止接收")
                break
            total_frame_count += 1  # 累加总帧数
            # 检查是否为关键帧并计数
            if feedback.color_frame.is_key_frame:
                keyframe_count += 1
                print(f"[关键帧 #{keyframe_count}] 收到关键帧 (总帧数: {total_frame_count})")
        stream_iterator.cancel()
        print(f"\n流接收结束 - 总帧数: {total_frame_count}, 关键帧数: {keyframe_count}, 关键帧占比: {keyframe_count/total_frame_count:.2%}")


    except grpc.RpcError as e:
        print(f"双向流通信错误: {e}")
    except Exception as e:
        print(f"处理流数据时发生错误: {e}")
    finally:
        # 关闭连接
        client.close()
        print("\n=== 双向流传感器数据测试完成 ===")

def test_dexhand():
    """测试DexHand功能（获取状态、设置角度和传感器数据）"""
    print("=== 测试DexHand功能 ===")

    # 初始化GRPC客户端（使用实际服务器地址）
    client = CMVRGrpcClient("192.168.0.222:50052")
    if not client.is_connected():
        print("连接服务器失败")
        return

    # 获取DexHand客户端（假设设备ID为"hand1"，根据实际设备ID修改）
    dexhand = client.get_dexhand("hand1")
    if not dexhand:
        print("获取DexHand客户端失败")
        client.close()
        return

    # 定义：字符串ID与手指名称的映射（与proto/服务端一致）
    dof_id_to_name = {
        "little_finger": "小拇指",
        "ring_finger": "无名指",
        "middle_finger": "中指",
        "index_finger": "食指",
        "thumb_bend": "大拇指弯曲",
        "thumb_rotate": "大拇指旋转"
    }
    # 有效字符串ID列表（用于后续判断）
    valid_dof_ids = set(dof_id_to_name.keys())

    # 1. 获取初始状态
    print("\n--- 获取初始状态 ---")
    error_code, state = dexhand.get_status()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print(f"初始化状态: 已初始化={state.is_initialized}")
        print("各自由度状态:")
        for hand in state.hands:
            # 匹配字符串ID对应的手指名称，未知ID显示原始值
            finger_name = dof_id_to_name.get(hand.dof_id, f"未知自由度({hand.dof_id})")
            print(f"  {finger_name} (ID:{hand.dof_id}): 角度={hand.angle}, 速度={hand.speed}, "
                  f"受力={hand.force}, 位置={hand.position}, 温度={hand.temperature}°C")
            if hand.error != 0:
                print(f"  警告: {finger_name} 存在故障: {hand.error_message}")
    else:
        print(f"获取初始状态失败，错误码: {error_code}")
        client.close()
        return

    # 1.1 获取初始传感器数据
    print("\n--- 获取初始传感器数据 ---")
    error_code, sensors = dexhand.get_sensor_data()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print("初始传感器数据摘要:")
        sensors.print_summary()  # 调用传感器数据摘要打印方法
        # 打印掌心和食指指端的传感器数据示例
        print(f"  掌心传感器数据形状: {sensors.palm.data.shape}")
        print(f"  食指指端传感器数据形状: {sensors.index_tip.data.shape}")
    else:
        print(f"获取初始传感器数据失败，错误码: {error_code}")

    # 2. 设置测试角度（小拇指、食指、大拇指弯曲等）
    print("\n--- 设置测试角度 ---")
    # 角度字典: 键为字符串类型自由度ID，值为0-1百分比（与set_angle函数参数要求一致）
    test_angles = {
        "little_finger": 0.9,   # 小拇指
        "ring_finger": 0.9,     # 无名指
        "middle_finger": 0.9,   # 中指
        "index_finger": 0.9,    # 食指
        "thumb_bend": 0.9,      # 大拇指弯曲
        "thumb_rotate": 0.9     # 大拇指旋转
    }

    # 打印设置信息（通过映射表显示中文名称）
    print("准备设置角度:")
    for dof_id, value in test_angles.items():
        finger_name = dof_id_to_name[dof_id]  # 已确保ID有效，直接获取名称
        print(f"  {finger_name} (ID:{dof_id}): {value*100}%")

    # 执行角度设置（直接传入字符串ID的字典）
    set_result = dexhand.set_angle(test_angles)
    if set_result == CMVRErrorCode.CMVR_SUCCESS:
        print("角度设置命令发送成功，等待执行器动作...")
        time.sleep(2)  # 等待执行器完成动作
    else:
        print(f"角度设置失败，错误码: {set_result}")
        client.close()
        return

    # 2.1 获取角度变化后的传感器数据
    print("\n--- 获取角度变化后传感器数据 ---")
    error_code, sensors_after = dexhand.get_sensor_data()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print("角度变化后传感器数据摘要:")
        # 对比食指指腹在动作前后的平均压力变化
        if hasattr(sensors, 'index_pad') and hasattr(sensors_after, 'index_pad'):
            avg_before = sensors.index_pad.data.mean() if sensors.index_pad.data.size > 0 else 0
            avg_after = sensors_after.index_pad.data.mean() if sensors_after.index_pad.data.size > 0 else 0
            print(f"  食指指腹平均压力变化: {avg_before:.2f} → {avg_after:.2f}")

        # 打印大拇指指端数据示例（前3个值）
        if hasattr(sensors_after, 'thumb_tip') and sensors_after.thumb_tip.data.size > 0:
            print(f"  大拇指指端部分数据: {sensors_after.thumb_tip.data.flatten()[:3]}...")
    else:
        print(f"获取角度变化后传感器数据失败，错误码: {error_code}")

    # 3. 获取设置后的状态
    print("\n--- 获取设置后状态 ---")
    error_code, state = dexhand.get_status()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print("设置后各自由度状态（仅显示已设置的自由度）:")
        for hand in state.hands:
            # 只打印我们设置过的自由度（字符串ID对比）
            if hand.dof_id in test_angles:
                finger_name = dof_id_to_name[hand.dof_id]
                print(f"  {finger_name} (ID:{hand.dof_id}): 新角度={hand.angle}, 当前位置={hand.position}")
    else:
        print(f"获取设置后状态失败，错误码: {error_code}")

    # 4. 恢复默认角度（复位操作）
    print("\n--- 恢复默认角度 ---")
    # 复位字典：键为字符串ID，值为默认百分比
    default_angles = {
        "little_finger": 0.1,
        "ring_finger": 0.1,
        "middle_finger": 0.1,
        "index_finger": 0.1,
        "thumb_bend": 0.1,
        "thumb_rotate": 0.8
    }

    set_result = dexhand.set_angle(default_angles)
    if set_result == CMVRErrorCode.CMVR_SUCCESS:
        print("默认角度恢复成功，等待复位完成...")
        time.sleep(2)
    else:
        print(f"默认角度恢复失败，错误码: {set_result}")

    # 4.1 获取复位后的传感器数据
    print("\n--- 获取复位后传感器数据 ---")
    error_code, sensors_reset = dexhand.get_sensor_data()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print("复位后传感器数据摘要:")
        # 对比掌心压力在复位前后的变化
        if hasattr(sensors_after, 'palm') and hasattr(sensors_reset, 'palm'):
            avg_after = sensors_after.palm.data.mean() if sensors_after.palm.data.size > 0 else 0
            avg_reset = sensors_reset.palm.data.mean() if sensors_reset.palm.data.size > 0 else 0
            print(f"  掌心平均压力变化: {avg_after:.2f} → {avg_reset:.2f}")
    else:
        print(f"获取复位后传感器数据失败，错误码: {error_code}")

    # 5. 最终状态确认
    print("\n--- 最终状态确认 ---")
    error_code, state = dexhand.get_status()
    if error_code == CMVRErrorCode.CMVR_SUCCESS:
        print(f"最终初始化状态: {state.is_initialized}")
        print("关键自由度最终位置:")
        # 遍历需要确认的自由度（字符串ID）
        for dof_id in test_angles.keys():
            for hand in state.hands:
                if hand.dof_id == dof_id:
                    finger_name = dof_id_to_name[dof_id]
                    print(f"  {finger_name} (ID:{dof_id}): 角度={hand.angle}, 位置={hand.position}")
    else:
        print(f"获取最终状态失败，错误码: {error_code}")

    # 关闭连接
    client.close()
    print("\n=== DexHand测试完成 ===")

def test_sensor_data_stream():
    """测试双向流传感器数据接口"""
    print("=== 测试双向流传感器数据接口 ===")

    # 初始化GRPC客户端
    client = CMVRGrpcClient("192.168.1.222:50052")
    if not client.is_connected():
        print("连接服务器失败")
        return

    # 获取DexHand客户端
    dexhand = client.get_dexhand("hand1")
    if not dexhand:
        print("获取DexHand客户端失败")
        client.close()
        return

    try:
        # 1. 启动传感器数据流
        print("\n--- 启动传感器数据流 ---")
        # 创建请求生成器（双向流不需要参数，发送空请求）
        def request_generator() -> Generator:
            # 发送初始请求启动流
            yield dexhand.create_stream_request()
            # 保持流连接，每5秒发送一次心跳（可选）
            try:
                while True:
                    time.sleep(5)
                    # 发送空请求保持连接
                    yield dexhand.create_stream_request()
            except GeneratorExit:
                print("请求生成器已关闭")
                return

        # 2. 接收并处理流数据
        print("开始接收传感器流数据（5秒后自动停止）...")
        start_time = time.time()
        stream_duration = 15  # 接收15秒数据

        # 调用双向流接口
        for feedback in dexhand.get_sensor_data_stream(request_generator()):
            # 检查是否超时
            if time.time() - start_time > stream_duration:
                print("\n达到预设接收时间，停止接收")
                break

            # # 检查反馈状态
            # if not feedback.header.success:
            #     print(f"数据流接收失败，错误码: {feedback.header.error_code}")
            #     continue

            # 处理传感器数据（转换为HandTactileSensors对象）
            error_code, sensors = dexhand.parse_sensor_stream_data(feedback)
            if error_code != CMVRErrorCode.CMVR_SUCCESS:
                print("传感器数据解析失败")
                continue

            # 打印传感器数据摘要（每秒打印一次）
            if int(time.time()) % 1 == 0:  # 控制打印频率
                print(f"\n--- 传感器数据 (时间: {time.time() - start_time:.2f}s) ---")
                sensors.print_summary()

                # 打印食指指端的实时数据示例
                if sensors.index_tip.data is not None and sensors.index_tip.data.size > 0:
                    print(f"食指指端平均压力: {sensors.index_tip.data.mean():.2f}")
                if sensors.palm.data is not None and sensors.palm.data.size > 0:
                    print(f"掌心平均压力: {sensors.palm.data.mean():.2f}")

    except grpc.RpcError as e:
        print(f"双向流通信错误: {e}")
    except Exception as e:
        print(f"处理流数据时发生错误: {e}")
    finally:
        # 关闭连接
        client.close()
        print("\n=== 双向流传感器数据测试完成 ===")

def test_speaker():
    """测试扬声器功能"""
    print("\n=== 测试扬声器功能 ===")

    # 初始化GRPC客户端
    client = CMVRGrpcClient("192.168.1.222:50060")
    if not client.is_connected():
        print("连接服务器失败")
        return

    speaker = client.get_speaker("spk1")
    if speaker is None:
        print("获取扬声器失败")
        return

    # 获取状态
    result, status = speaker.get_status()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print(f"扬声器状态: 初始化={status.is_initialized}, 运行中={status.is_running}")
    else:
        print(f"获取扬声器状态失败，错误码: {result}")

    # 设置音量
    result = speaker.set_volume(80)  # 80%音量
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("设置音量成功")
    else:
        print(f"设置音量失败，错误码: {result}")

    # 获取音量
    result, volume = speaker.get_volume()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print(f"当前音量: {volume}%")
    else:
        print(f"获取音量失败，错误码: {result}")

    # 播放音频 (需要替换为实际存在的音频文件路径)
    audio_path = "/home/share/assets/upload/员工女_车内温度有点低，是否需要调高空调？.wav"
    print(f"尝试播放音频: {audio_path}")
    result = speaker.play_audio(audio_path)
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("开始播放音频")

        # 等待3秒
        time.sleep(3)

        # 暂停播放
        result = speaker.pause_audio()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("暂停播放")

            # 等待1秒
            time.sleep(1)

            # 继续播放
            result = speaker.resume_audio()
            if result == CMVRErrorCode.CMVR_SUCCESS:
                print("继续播放")

                # 等待2秒
                time.sleep(2)

                # 停止播放
                result = speaker.stop_audio()
                if result == CMVRErrorCode.CMVR_SUCCESS:
                    print("停止播放")
                else:
                    print(f"停止播放失败，错误码: {result}")
            else:
                print(f"继续播放失败，错误码: {result}")
        else:
            print(f"暂停播放失败，错误码: {result}")
    else:
        print(f"播放音频失败，错误码: {result}")

def test_microphone():
    """测试麦克风功能"""
    print("\n=== 测试麦克风功能 ===")
    # 初始化GRPC客户端
    client = CMVRGrpcClient("192.168.1.222:50060")
    if not client.is_connected():
        print("连接服务器失败")
        return

    microphone = client.get_micphone("mic2")
    if microphone is None:
        print("获取麦克风失败")
        return

    # 获取状态
    result, status = microphone.get_status()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print(f"麦克风状态: 初始化={status.is_initialized}, 录音中={status.is_recording}")
    else:
        print(f"获取麦克风状态失败，错误码: {result}")

    # 开始录音
    audio_path = "/home/linbo/Data/Audio/recorded_audio.mp3"
    result = microphone.start_record(audio_path)
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print(f"开始录音，保存到: {audio_path}")

        # 录音5秒
        time.sleep(5)

        # 停止录音
        result = microphone.stop_record()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("停止录音")
        else:
            print(f"停止录音失败，错误码: {result}")
    else:
        print(f"开始录音失败，错误码: {result}")

def test_humanoid_robot():
    """测试机器人功能"""
    print("\n=== 测试机器人功能 ===")
    # 初始化GRPC客户端
    client = CMVRGrpcClient("192.168.0.222:50052")
    if not client.is_connected():
        print("连接服务器失败")
        return

    humanoidRobot = client.get_humanoid_robot("hc01")
    if humanoidRobot is None:
        print("获取机器人失败")
        return

    

    # 使能
    # print("\n1. 开启机器人使能...")
    # result = humanoidRobot.torqueOn()
    # if result == CMVRErrorCode.CMVR_SUCCESS:
    #     print("✅ 使能开启成功")
    # else:
    #     print(f"❌ 使能开启失败，错误码: {result}")
    #     return

    # 等待使能生效
    import time
    time.sleep(1)

    try:
        # 获取初始状态
        print("\n2. 获取初始关节状态...")
        # 获取状态
        result, joint_states = humanoidRobot.get_joint_states()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print(f"成功获取 {len(joint_states)} 个关节状态组")
            
            for i, status in enumerate(joint_states):
                print(f"\n=== 关节状态组 {i+1} ===")
                print(f"时间戳: {status.timestamp:.3f}s")
                print(f"关节数量: {len(status.name)}")
                
                if len(status.name) > 0:
                    print("\n关节详情:")
                    print(f"{'关节名称':<20} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}")
                    print("-" * 55)
                    
                    for j, name in enumerate(status.name):
                        pos = status.position[j] if j < len(status.position) else 0.0
                        vel = status.velocity[j] if j < len(status.velocity) else 0.0
                        eff = status.effort[j] if j < len(status.effort) else 0.0
                        
                        print(f"{name:<20} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}")
                else:
                    print("该状态组没有关节数据")
                    
            # 打印所有状态的汇总信息
            print(f"\n=== 汇总信息 ===")
            total_joints = sum(len(state.name) for state in joint_states)
            timestamps = [state.timestamp for state in joint_states]
            print(f"总关节数: {total_joints}")
            print(f"时间戳范围: {min(timestamps):.3f}s - {max(timestamps):.3f}s")
            
        else:
            print(f"获取关节状态失败，错误码: {result}")

        return
        # 测试moveJ - 关节空间运动
        print("\n3. 测试关节空间运动 (moveJ)...")

        # 创建关节命令列表,回到零位
        joint_commands = [
            JointCmd(joint_name="L_SHOULDER_P", rad=0, vel=0.8),
            JointCmd(joint_name="L_SHOULDER_R", rad=0, vel=0.8),
            JointCmd(joint_name="L_SHOULDER_Y", rad=0, vel=0.8),
            JointCmd(joint_name="L_ELBOW_R", rad=0, vel=0.8),
            JointCmd(joint_name="L_WRIST_P", rad=0, vel=0.8),
            JointCmd(joint_name="L_WRIST_Y", rad=0, vel=0.8),
            JointCmd(joint_name="L_WRIST_R", rad=0, vel=0.8),

            JointCmd(joint_name="R_SHOULDER_P", rad=0, vel=0.8),
            JointCmd(joint_name="R_SHOULDER_R", rad=0, vel=0.8),
            JointCmd(joint_name="R_SHOULDER_Y", rad=0, vel=0.8),
            JointCmd(joint_name="R_ELBOW_R", rad=0, vel=0.8),
            JointCmd(joint_name="R_WRIST_P", rad=0, vel=0.8),
            JointCmd(joint_name="R_WRIST_Y", rad=0, vel=0.8),
            JointCmd(joint_name="R_WRIST_R", rad=0, vel=0.8),

            JointCmd(joint_name="WAIST_P", rad=0, vel=0.8),
            JointCmd(joint_name="WAIST_Y", rad=0, vel=0.8)
        ]
        
        result, _ = humanoidRobot.moveJ(joint_commands, overall_vel=0.8, overall_acc=0.4)
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("✅ moveJ 命令发送成功")
            time.sleep(3)  # 等待运动完成
        else:
            print(f"❌ moveJ 命令发送失败，错误码: {result}")

        # 获取运动后的状态
        print("\n   获取moveJ后的关节状态...")
        result, status = humanoidRobot.get_joint_state()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print(f"   运动后时间戳: {status.timestamp:.3f}s")
            print(f"   关节数量: {len(status.name)}")

        # 测试moveL - 直线空间运动
        print("\n4. 测试直线空间运动 (moveL)...")
        # 创建目标位姿 (相对于当前位姿的小幅移动)
        target_pose = Pose3D(x=0.05, y=0.0, z=0.0, rx=0.0, ry=0.0, rz=0.0)
        
        result, _ = humanoidRobot.moveL(ee_link="R_WRIST_R", target_pose=target_pose, 
                                       vel=0.05, acc=0.2)
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("✅ moveL 命令发送成功")
            time.sleep(3)  # 等待运动完成
        else:
            print(f"❌ moveL 命令发送失败，错误码: {result}")

        # 测试speedJ - 关节速度控制
        print("\n5. 测试关节速度控制 (speedJ)...")
        result, error_msg = humanoidRobot.speedJ(joint_name="R_SHOULDER_P", 
                                                velocity=0.1, 
                                                direction=RobotJointIndexDirection.FORWARD,
                                                acc=0.3)
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("✅ speedJ 命令发送成功")
            time.sleep(2)  # 运行2秒
            # 停止运动
            stop_result, _ = humanoidRobot.speedJ(joint_name="R_SHOULDER_P", 
                                                 velocity=0.0,
                                                 direction=RobotJointIndexDirection.FORWARD,
                                                 acc=0.3)
            print("   停止speedJ运动")
        else:
            print(f"❌ speedJ 命令发送失败，错误码: {result}")
            if error_msg:
                print(f"   错误信息: {error_msg}")

        # 测试speedL - 笛卡尔速度控制
        print("\n6. 测试笛卡尔速度控制 (speedL)...")
        result, error_msg = humanoidRobot.speedL(ee_link="R_WRIST_R", 
                                                velocity=0.02, 
                                                cartesian=RobotCartesian.X,
                                                direction=RobotJointIndexDirection.FORWARD,
                                                acc=0.1)
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("✅ speedL 命令发送成功")
            time.sleep(2)  # 运行2秒
            # 停止运动
            stop_result, _ = humanoidRobot.speedL(ee_link="R_WRIST_R", 
                                                 velocity=0.0,
                                                 cartesian=RobotCartesian.X,
                                                 direction=RobotJointIndexDirection.FORWARD,
                                                 acc=0.1)
            print("   停止speedL运动")
        else:
            print(f"❌ speedL 命令发送失败，错误码: {result}")
            if error_msg:
                print(f"   错误信息: {error_msg}")

        # 获取最终状态
        print("\n7. 获取最终关节状态...")
        result, status = humanoidRobot.get_joint_state()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("\n=== 最终关节状态信息 ===")
            print(f"时间戳: {status.timestamp:.3f}s")
            print(f"关节数量: {len(status.name)}")
            
            if len(status.name) > 0:
                print("\n关节详情:")
                print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}")
                print("-" * 50)
                
                for i, name in enumerate(status.name):
                    pos = status.position[i] if i < len(status.position) else 0.0
                    vel = status.velocity[i] if i < len(status.velocity) else 0.0
                    eff = status.effort[i] if i < len(status.effort) else 0.0
                    
                    print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}")
            else:
                print("没有关节数据")
        else:
            print(f"获取最终状态失败，错误码: {result}")

    except Exception as e:
        print(f"\n❌ 测试过程中发生异常: {e}")
        import traceback
        traceback.print_exc()

    finally:
        # 去使能
        print("\n8. 关闭机器人使能...")
        result = humanoidRobot.torqueOff()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("✅ 使能关闭成功")
        else:
            print(f"❌ 使能关闭失败，错误码: {result}")

        print("\n=== 机器人功能测试完成 ===")

if __name__ == "__main__":
    # test_biohead()
    # test_camera()
    # test_rgb_image_stream()
    test_dexhand()
    # test_sensor_data_stream()
    # test_speaker()
    # test_microphone()
    # test_humanoid_robot()