74 lines
2.5 KiB
Python
74 lines
2.5 KiB
Python
|
import sys
|
||
|
|
import time
|
||
|
|
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):
|
||
|
|
"""Continuous client to fetch robot joint states"""
|
||
|
|
|
||
|
|
def send(self, interval=0.5, device_id="hc01"):
|
||
|
|
"""Continuously fetch joint states and print in fixed order"""
|
||
|
|
try:
|
||
|
|
while True:
|
||
|
|
# Construct request
|
||
|
|
req = pb.JointRequest()
|
||
|
|
req.header.device_id = device_id
|
||
|
|
timestamp = timestamp_pb2.Timestamp()
|
||
|
|
timestamp.GetCurrentTime()
|
||
|
|
req.header.timestamp.CopyFrom(timestamp)
|
||
|
|
|
||
|
|
# Call RPC with timeout
|
||
|
|
try:
|
||
|
|
resp = self.stub.getJointState(req, timeout=10)
|
||
|
|
except Exception as e:
|
||
|
|
print(f"RPC call failed: {e}")
|
||
|
|
time.sleep(interval)
|
||
|
|
continue
|
||
|
|
|
||
|
|
# Fixed order for printing
|
||
|
|
joint_order = [
|
||
|
|
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
||
|
|
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
|
||
|
|
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||
|
|
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
|
||
|
|
"HEAD_P", "HEAD_Y","HEAD_R"
|
||
|
|
]
|
||
|
|
|
||
|
|
|
||
|
|
# Map response to dictionary
|
||
|
|
joint_dict = {}
|
||
|
|
for state in resp.state:
|
||
|
|
for name, pos in zip(state.name, state.position):
|
||
|
|
joint_dict[name] = round(pos, 12)
|
||
|
|
|
||
|
|
joint_list = [{"joint_name": name, "rad": joint_dict.get(name, 0.0)}
|
||
|
|
for name in joint_order]
|
||
|
|
|
||
|
|
# Print timestamp
|
||
|
|
print(f"[{time.strftime('%H:%M:%S')}]")
|
||
|
|
# Print each joint, last one without comma
|
||
|
|
for i, j in enumerate(joint_list):
|
||
|
|
if i < len(joint_list) - 1:
|
||
|
|
print(f"{j},")
|
||
|
|
else:
|
||
|
|
print(f"{j}")
|
||
|
|
|
||
|
|
time.sleep(interval)
|
||
|
|
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
print("\nStopped fetching joint states.")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
client = GetJointStateClient()
|
||
|
|
try:
|
||
|
|
client.send(interval=0.5)
|
||
|
|
finally:
|
||
|
|
client.close()
|