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 humanoid_robot_command_pb2 as pb
from cmvr.api import humanoid_robot_service_pb2_grpc as rpc


class TorqueOnClient(RobotClientBase):
    """发送 TorqueOn 指令客户端"""

    def send(self, device_id="hc01"):
        # 构造请求头
        req = common_pb2.CommandHeader.Request()
        req.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        req.timestamp.CopyFrom(ts)

        # 调用 RPC
        try:
            resp = self.stub.torqueOn(req, timeout=10)
            print("✅ TorqueOn RPC 调用成功")
            print(f"Success: {resp.success}")
            print(f"Error message: {resp.error_message}")
            print(f"Timestamp: {resp.timestamp.seconds}")
        except Exception as e:
            print("❌ TorqueOn RPC 调用失败:", e)


if __name__ == "__main__":
    # 初始化客户端
    client = TorqueOnClient()

    # 发送 TorqueOn 指令
    client.send(device_id="hc01")

    # 关闭客户端
    client.close()
