89 lines
3.2 KiB
Python
89 lines
3.2 KiB
Python
import grpc
|
|
from typing import Tuple
|
|
|
|
from . import generated
|
|
from .enums import CMVRErrorCode
|
|
from .models import DexHandState, RH56DFTPDexHand
|
|
|
|
class DexHandClient:
|
|
"""灵巧手客户端"""
|
|
|
|
def __init__(self, device_id: str, stub):
|
|
self.device_id = device_id
|
|
self.stub = stub
|
|
|
|
def _create_command_header(self):
|
|
"""创建命令头"""
|
|
header = generated.common_pb2.CommandHeader.Request()
|
|
header.device_id = self.device_id
|
|
header.timestamp.GetCurrentTime()
|
|
return header
|
|
|
|
def get_status(self) -> Tuple[CMVRErrorCode, DexHandState]:
|
|
"""获取灵巧手状态"""
|
|
try:
|
|
request = generated.dexhand_service_pb2.GetDexHandStateCommand.Request()
|
|
request.header.CopyFrom(self._create_command_header())
|
|
|
|
response = self.stub.GetStatus(request)
|
|
|
|
if not response.header.success:
|
|
return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState()
|
|
|
|
state = DexHandState(
|
|
is_initialized=response.state.is_initialized
|
|
)
|
|
|
|
# 填充手指状态
|
|
for i, hand_state in enumerate(response.state.hands):
|
|
if i < len(state.hands):
|
|
state.hands[i] = RH56DFTPDexHand(
|
|
angle=hand_state.angle,
|
|
speed=hand_state.speed,
|
|
force=hand_state.force,
|
|
position=hand_state.position,
|
|
current=hand_state.current,
|
|
temperature=hand_state.temperature,
|
|
error=hand_state.error,
|
|
error_message=list(hand_state.error_message)
|
|
)
|
|
|
|
return CMVRErrorCode.CMVR_SUCCESS, state
|
|
|
|
except grpc.RpcError as e:
|
|
print(f"获取灵巧手状态失败: {e}")
|
|
return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState()
|
|
|
|
def set_position(self, dof_id: int, value: float) -> CMVRErrorCode:
|
|
"""设置位置"""
|
|
try:
|
|
request = generated.dexhand_service_pb2.SetDexHandPositionsCommand.Request()
|
|
request.header.CopyFrom(self._create_command_header())
|
|
|
|
# 添加自由度值
|
|
freedom_value = request.values.add()
|
|
freedom_value.id = dof_id
|
|
freedom_value.value = value
|
|
|
|
response = self.stub.SetDexHandPos(request)
|
|
|
|
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
|
|
|
except grpc.RpcError as e:
|
|
print(f"设置位置失败: {e}")
|
|
return CMVRErrorCode.CMVR_RPC_FAILED
|
|
|
|
def set_preset_act(self, preset_act_id: int) -> CMVRErrorCode:
|
|
"""设置预设动作"""
|
|
try:
|
|
request = generated.dexhand_service_pb2.SetDexHandPresetActCommand.Request()
|
|
request.header.CopyFrom(self._create_command_header())
|
|
request.presetactid = preset_act_id
|
|
|
|
response = self.stub.SetDexHandPresetAct(request)
|
|
|
|
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
|
|
|
except grpc.RpcError as e:
|
|
print(f"设置预设动作失败: {e}")
|
|
return CMVRErrorCode.CMVR_RPC_FAILED |