unitree_sdk2_python/unitree_sdk2py/rpc/client_base.py at 8a835a6c18bc8a048b728819b01b8afe446f69cd · unitreerobotics/unitree_sdk2_python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
import time

from ..idl.unitree_api.msg.dds_ import Request_ as Request
from ..idl.unitree_api.msg.dds_ import RequestHeader_ as RequestHeader
from ..idl.unitree_api.msg.dds_ import RequestLease_ as RequestLease
from ..idl.unitree_api.msg.dds_ import RequestIdentity_ as RequestIdentity
from ..idl.unitree_api.msg.dds_ import RequestPolicy_ as RequestPolicy

from ..utils.future import FutureResult

from .client_stub import ClientStub
from .internal import *


"""
" class ClientBase
"""
class ClientBase:
    def __init__(self, serviceName: str):
        self.__timeout = 1.0
        self.__stub = ClientStub(serviceName)
        self.__stub.Init()

    def SetTimeout(self, timeout: float):
        self.__timeout = timeout

    def _CallBase(self, apiId: int, parameter: str, proirity: int = 0, leaseId: int = 0):
        # print("[CallBase] call apiId:", apiId, ", proirity:", proirity, ", leaseId:", leaseId)
        header = self.__SetHeader(apiId, leaseId, proirity, False)
        request = Request(header, parameter, [])

        future = self.__stub.SendRequest(request, self.__timeout)
        if future is None:
            return RPC_ERR_CLIENT_SEND, None

        result = future.GetResult(self.__timeout)

        if result.code != FutureResult.FUTURE_SUCC:
            self.__stub.RemoveFuture(request.header.identity.id)
            code = RPC_ERR_CLIENT_API_TIMEOUT if result.code == FutureResult.FUTUTE_ERR_TIMEOUT else RPC_ERR_UNKNOWN
            return code, None

        response = result.value

        if response.header.identity.api_id != apiId:
            return RPC_ERR_CLIENT_API_NOT_MATCH, None
        else:
            return response.header.status.code, response.data

    def _CallNoReplyBase(self, apiId: int, parameter: str, proirity: int, leaseId: int):
        header = self.__SetHeader(apiId, leaseId, proirity, True)
        request = Request(header, parameter, [])

        if self.__stub.Send(request, self.__timeout):
            return 0
        else:
            return RPC_ERR_CLIENT_SEND

    def _CallBinaryBase(self, apiId: int, parameter: list, proirity: int, leaseId: int):
        header = self.__SetHeader(apiId, leaseId, proirity, False)
        request = Request(header, "", parameter)

        future = self.__stub.SendRequest(request, self.__timeout)
        if future is None:
            return RPC_ERR_CLIENT_SEND, None

        result = future.GetResult(self.__timeout)
        if result.code != FutureResult.FUTURE_SUCC:
            self.__stub.RemoveFuture(request.header.identity.id)
            code = RPC_ERR_CLIENT_API_TIMEOUT if result.code == FutureResult.FUTUTE_ERR_TIMEOUT else RPC_ERR_UNKNOWN
            return code, None

        response = result.value

        if response.header.identity.api_id != apiId:
            return RPC_ERR_CLIENT_API_NOT_MATCH, None
        else:
            return response.header.status.code, response.binary

    def _CallBinaryNoReplyBase(self, apiId: int, parameter: list, proirity: int, leaseId: int):
        header = self.__SetHeader(apiId, leaseId, proirity, True)
        request = Request(header, "", parameter)

        if self.__stub.Send(request, self.__timeout):
            return 0
        else:
            return RPC_ERR_CLIENT_SEND

    def _CallParamBinaryBase(self, apiId: int, parameter: str, binary: list, proirity: int = 0, leaseId: int = 0):
        header = self.__SetHeader(apiId, leaseId, proirity, False)
        request = Request(header, parameter, binary)

        future = self.__stub.SendRequest(request, self.__timeout)
        if future is None:
            return RPC_ERR_CLIENT_SEND, None

        result = future.GetResult(self.__timeout)

        if result.code != FutureResult.FUTURE_SUCC:
            self.__stub.RemoveFuture(request.header.identity.id)
            code = RPC_ERR_CLIENT_API_TIMEOUT if result.code == FutureResult.FUTUTE_ERR_TIMEOUT else RPC_ERR_UNKNOWN
            return code, None

        response = result.value

        if response.header.identity.api_id != apiId:
            return RPC_ERR_CLIENT_API_NOT_MATCH, None
        else:
            return response.header.status.code, response.data

    def __SetHeader(self, apiId: int, leaseId: int, priority: int, noReply: bool):
        identity = RequestIdentity(time.monotonic_ns(), apiId)
        lease = RequestLease(leaseId)
        policy = RequestPolicy(priority, noReply)
        return RequestHeader(identity, lease, policy)