63 lines
2.0 KiB
Python
63 lines
2.0 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 GetPoseClient(RobotClientBase):
|
|
"""Continuous client to fetch robot pose"""
|
|
|
|
def send(self, interval=0.5, device_id="hc01", base_link="PELVIS_S", ee_link="R_FINGER_TIP"):
|
|
"""Continuously fetch pose and print in a fixed order"""
|
|
try:
|
|
while True:
|
|
# Construct request
|
|
req = pb.GetPose.Request()
|
|
req.header.device_id = device_id
|
|
req.base_link = base_link
|
|
req.ee_link = ee_link
|
|
timestamp = timestamp_pb2.Timestamp()
|
|
timestamp.GetCurrentTime()
|
|
req.header.timestamp.CopyFrom(timestamp)
|
|
|
|
# Call RPC with timeout
|
|
try:
|
|
resp = self.stub.getPose(req, timeout=10)
|
|
except Exception as e:
|
|
print(f"RPC call failed: {e}")
|
|
time.sleep(interval)
|
|
continue
|
|
|
|
# Extract the pose information from the response
|
|
pose = resp.pose
|
|
pose_data = {
|
|
"x": round(pose.x, 6),
|
|
"y": round(pose.y, 6),
|
|
"z": round(pose.z, 6),
|
|
"rx": round(pose.rx, 6),
|
|
"ry": round(pose.ry, 6),
|
|
"rz": round(pose.rz, 6),
|
|
}
|
|
|
|
# Print timestamp
|
|
print(f"[{time.strftime('%H:%M:%S')}]")
|
|
# Print pose information
|
|
print(f"Pose: {pose_data}")
|
|
|
|
time.sleep(interval)
|
|
|
|
except KeyboardInterrupt:
|
|
print("\nStopped fetching pose.")
|
|
|
|
if __name__ == "__main__":
|
|
client = GetPoseClient()
|
|
try:
|
|
client.send(interval=0.5)
|
|
finally:
|
|
client.close()
|