72 lines
2.4 KiB
Python
72 lines
2.4 KiB
Python
import sys
|
||
import csv
|
||
import math
|
||
from google.protobuf import timestamp_pb2
|
||
|
||
sys.path.append("../generated")
|
||
|
||
from generated.cmvr.api import humanoid_robot_service_pb2_grpc as rpc
|
||
from generated.cmvr.api import humanoid_robot_command_pb2 as pb
|
||
from clients.base_client import RobotClientBase
|
||
|
||
|
||
class GetJointStateClient(RobotClientBase):
|
||
"""客户端：获取机械臂关节数据并保存到CSV"""
|
||
|
||
def fetch_and_save_joint_states(self, device_id="hc01"):
|
||
"""获取关节数据一次并保存到CSV文件"""
|
||
try:
|
||
# 构建请求
|
||
req = pb.JointRequest()
|
||
req.header.device_id = device_id
|
||
timestamp = timestamp_pb2.Timestamp()
|
||
timestamp.GetCurrentTime()
|
||
req.header.timestamp.CopyFrom(timestamp)
|
||
|
||
# 调用RPC获取数据
|
||
try:
|
||
resp = self.stub.getJointState(req, timeout=10)
|
||
except Exception as e:
|
||
print(f"RPC调用失败: {e}")
|
||
return
|
||
|
||
# 定义需要保存的关节名称
|
||
joint_order = [
|
||
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
|
||
]
|
||
|
||
# 将响应数据映射到字典
|
||
joint_dict = {}
|
||
for state in resp.state:
|
||
for name, pos in zip(state.name, state.position):
|
||
joint_dict[name] = round(pos, 6)
|
||
|
||
# 将角度从弧度转换为度
|
||
joint_list = [{"joint_name": name, "deg": round(joint_dict.get(name, 0.0) * 180 / math.pi, 10)}
|
||
for name in joint_order]
|
||
|
||
# 准备保存到CSV的数据行
|
||
data_row = [joint['deg'] for joint in joint_list]
|
||
|
||
# 以追加模式打开CSV文件并写入数据
|
||
with open("joint_states_10_23.csv", mode="a", newline="") as file:
|
||
writer = csv.writer(file)
|
||
# 如果文件为空，写入表头
|
||
if file.tell() == 0:
|
||
writer.writerow(joint_order) # 表头
|
||
writer.writerow(data_row) # 写入数据行
|
||
|
||
print(f"数据已保存: {data_row}")
|
||
|
||
except KeyboardInterrupt:
|
||
print("\n停止获取关节数据。")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
client = GetJointStateClient()
|
||
try:
|
||
client.fetch_and_save_joint_states()
|
||
finally:
|
||
client.close()
|