61 lines
1.9 KiB
Python
61 lines
1.9 KiB
Python
import argparse
|
||
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 CalibrateZeroQClient(RobotClientBase):
|
||
"""Client to send calibrateZeroQ command"""
|
||
|
||
def send(self, device_id="right_arm", joint_name="xxx"):
|
||
# Construct request header
|
||
header = common_pb2.CommandHeader.Request()
|
||
header.device_id = device_id
|
||
ts = timestamp_pb2.Timestamp()
|
||
ts.GetCurrentTime()
|
||
header.timestamp.CopyFrom(ts)
|
||
|
||
# Construct CalibrateZeroQ request
|
||
req = pb.CalibrateZeroQ.Request(
|
||
header=header,
|
||
joint_name=joint_name
|
||
)
|
||
|
||
# Call RPC
|
||
try:
|
||
resp = self.stub.calibrateZeroQ(req, timeout=10)
|
||
print("calibrateZeroQ RPC call succeeded")
|
||
print(f"Success: {resp.success}")
|
||
print(f"Error message: {resp.error_message}")
|
||
print(f"Timestamp: {resp.timestamp.seconds}")
|
||
except Exception as e:
|
||
print("calibrateZeroQ RPC call failed:", e)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
# 创建命令行参数解析器
|
||
parser = argparse.ArgumentParser(description='Send calibrateZeroQ command to robot')
|
||
parser.add_argument('--device_id', type=str, default='right_arm',
|
||
help='Device ID (default: right_arm)')
|
||
parser.add_argument('--joint_name', type=str, required=True,
|
||
help='Joint name to calibrate zero position')
|
||
|
||
# 解析命令行参数
|
||
args = parser.parse_args()
|
||
|
||
# 初始化客户端
|
||
client = CalibrateZeroQClient()
|
||
|
||
# 发送calibrateZeroQ命令，使用命令行传入的参数
|
||
client.send(device_id=args.device_id, joint_name=args.joint_name)
|
||
|
||
# 关闭客户端
|
||
client.close()
|